DGMSolver.cpp

Go to the documentation of this file.

#include "moab/DiscreteGeometry/DGMSolver.hpp"
#include "moab/ErrorHandler.hpp"
#include <iostream>
#include <cassert>
#include <vector>
#include <limits>
#include <cmath>
#include <algorithm>
 
namespace moab
{
 
/* This class implements the lowest level solvers required by polynomial fitting for high-order
 * reconstruction. An underlying assumption of the matrices passed are that they are given in a
 * column-major form. So, the first mrows values of V is the first column, and so on. This
 * assumption is made because most of the operations are column-based in the current scenario.
 * */
 
unsigned int DGMSolver::nchoosek( unsigned int n, unsigned int k )
{
 if( k > n )
 {
 return 0;
 }
 unsigned long long ans = 1;
 if( k > ( n >> 1 ) )
 {
 k = n - k;
 }
 for( unsigned int i = 1; i <= k; ++i )
 {
 ans *= n--;
 ans /= i;
 if( ans > std::numeric_limits< unsigned int >::max() )
 {
 return 0;
 }
 }
 return ans;
}
 
unsigned int DGMSolver::compute_numcols_vander_multivar( unsigned int kvars, unsigned int degree )
{
 unsigned int mcols = 0;
 for( unsigned int i = 0; i <= degree; ++i )
 {
 unsigned int temp = nchoosek( kvars - 1 + i, kvars - 1 );
 if( !temp )
 {
 std::cout << "overflow to compute nchoosek n= " << kvars - 1 + i << " k= " << kvars - 1 << std::endl;
 return 0;
 }
 mcols += temp;
 }
 return mcols;
}
 
void DGMSolver::gen_multivar_monomial_basis( const int kvars,
 const double* vars,
 const int degree,
 std::vector< double >& basis )
{
 unsigned int len = compute_numcols_vander_multivar( kvars, degree );
 basis.reserve( len - basis.capacity() + basis.size() );
 size_t iend = basis.size();
#ifndef NDEBUG
 size_t istr = basis.size();
#endif
 basis.push_back( 1 );
 ++iend;
 if( !degree )
 {
 return;
 }
 std::vector< size_t > varspos( kvars );
 // degree 1
 for( int ivar = 0; ivar < kvars; ++ivar )
 {
 basis.push_back( vars[ivar] );
 varspos[ivar] = iend++;
 }
 // degree 2 to degree
 for( int ideg = 2; ideg <= degree; ++ideg )
 {
 size_t preend = iend;
 for( int ivar = 0; ivar < kvars; ++ivar )
 {
 size_t varpreend = iend;
 for( size_t ilast = varspos[ivar]; ilast < preend; ++ilast )
 {
 basis.push_back( vars[ivar] * basis[ilast] );
 ++iend;
 }
 varspos[ivar] = varpreend;
 }
 }
 assert( len == iend - istr );
}
 
void DGMSolver::gen_vander_multivar( const int mrows,
 const int kvars,
 const double* us,
 const int degree,
 std::vector< double >& V )
{
 unsigned int ncols = compute_numcols_vander_multivar( kvars, degree );
 V.reserve( mrows * ncols - V.capacity() + V.size() );
 size_t istr = V.size(), icol = 0;
 // add ones, V is stored in an single array, elements placed in columnwise order
 for( int irow = 0; irow < mrows; ++irow )
 {
 V.push_back( 1 );
 }
 ++icol;
 if( !degree )
 {
 return;
 }
 std::vector< size_t > varspos( kvars );
 // degree 1
 for( int ivar = 0; ivar < kvars; ++ivar )
 {
 for( int irow = 0; irow < mrows; ++irow )
 {
 V.push_back( us[irow * kvars + ivar] ); // us stored in row-wise
 }
 varspos[ivar] = icol++;
 }
 // from 2 to degree
 for( int ideg = 2; ideg <= degree; ++ideg )
 {
 size_t preendcol = icol;
 for( int ivar = 0; ivar < kvars; ++ivar )
 {
 size_t varpreend = icol;
 for( size_t ilast = varspos[ivar]; ilast < preendcol; ++ilast )
 {
 for( int irow = 0; irow < mrows; ++irow )
 {
 V.push_back( us[irow * kvars + ivar] * V[istr + irow + ilast * mrows] );
 }
 ++icol;
 }
 varspos[ivar] = varpreend;
 }
 }
 assert( icol == ncols );
}
 
void DGMSolver::rescale_matrix( int mrows, int ncols, double* V, double* ts )
{
 // This function rescales the input matrix using the norm of each column.
 double* v = new double[mrows];
 for( int i = 0; i < ncols; i++ )
 {
 for( int j = 0; j < mrows; j++ )
 v[j] = V[mrows * i + j];
 
 // Compute norm of the column vector
 double w = vec_2norm( mrows, v );
 
 if( fabs( w ) == 0 )
 ts[i] = 1;
 else
 {
 ts[i] = w;
 for( int j = 0; j < mrows; j++ )
 V[mrows * i + j] = V[mrows * i + j] / ts[i];
 }
 }
 delete[] v;
}
 
void DGMSolver::compute_qtransposeB( int mrows, int ncols, const double* Q, int bncols, double* bs )
{
 for( int k = 0; k < ncols; k++ )
 {
 for( int j = 0; j < bncols; j++ )
 {
 double t2 = 0;
 for( int i = k; i < mrows; i++ )
 t2 += Q[mrows * k + i] * bs[mrows * j + i];
 t2 = t2 + t2;
 
 for( int i = k; i < mrows; i++ )
 bs[mrows * j + i] -= t2 * Q[mrows * k + i];
 }
 }
}
 
void DGMSolver::qr_polyfit_safeguarded( const int mrows, const int ncols, double* V, double* D, int* rank )
{
 double tol = 1e-8;
 *rank = ncols;
 double* v = new double[mrows];
 
 for( int k = 0; k < ncols; k++ )
 {
 int nv = mrows - k;
 
 for( int j = 0; j < nv; j++ )
 v[j] = V[mrows * k + ( j + k )];
 
 double t2 = 0;
 
 for( int j = 0; j < nv; j++ )
 t2 = t2 + v[j] * v[j];
 
 double t = sqrt( t2 );
 double vnrm = 0;
 
 if( v[0] >= 0 )
 {
 vnrm = sqrt( 2 * ( t2 + v[0] * t ) );
 v[0] = v[0] + t;
 }
 else
 {
 vnrm = sqrt( 2 * ( t2 - v[0] * t ) );
 v[0] = v[0] - t;
 }
 
 if( vnrm > 0 )
 {
 for( int j = 0; j < nv; j++ )
 v[j] = v[j] / vnrm;
 }
 
 for( int j = k; j < ncols; j++ )
 {
 t2 = 0;
 for( int i = 0; i < nv; i++ )
 t2 = t2 + v[i] * V[mrows * j + ( i + k )];
 
 t2 = t2 + t2;
 
 for( int i = 0; i < nv; i++ )
 V[mrows * j + ( i + k )] = V[mrows * j + ( i + k )] - t2 * v[i];
 }
 
 D[k] = V[mrows * k + k];
 
 for( int i = 0; i < nv; i++ )
 V[mrows * k + ( i + k )] = v[i];
 
 if( ( fabs( D[k] ) ) < tol && ( *rank == ncols ) )
 {
 *rank = k;
 break;
 }
 }
 
 delete[] v;
}
 
void DGMSolver::backsolve( int mrows, int ncols, double* R, int bncols, double* bs, double* ws )
{
#if 0
 std::cout.precision(16);
 std::cout<<"Before backsolve "<<std::endl;
 std::cout<<" V = "<<std::endl;
 for (int k=0; k< ncols; k++){
 for (int j=0; j<mrows; ++j){
 std::cout<<R[mrows*k+j]<<std::endl;
 }
 std::cout<<std::endl;
 }
 std::cout<<std::endl;
 
 //std::cout<<"#pnts = "<<mrows<<std::endl;
 std::cout<<"bs = "<<std::endl;
 std::cout<<std::endl;
 for (int k=0; k< bncols; k++){
 for (int j=0; j<mrows; ++j){
 std::cout<<" "<<bs[mrows*k+j]<<std::endl;
 }
 }
 std::cout<<std::endl;
#endif
 
 for( int k = 0; k < bncols; k++ )
 {
 for( int j = ncols - 1; j >= 0; j-- )
 {
 for( int i = j + 1; i < ncols; ++i )
 bs[mrows * k + j] = bs[mrows * k + j] - R[mrows * i + j] * bs[mrows * k + i];
 
 assert( R[mrows * j + j] != 0 );
 bs[mrows * k + j] = bs[mrows * k + j] / R[mrows * j + j];
 }
 }
 
 for( int k = 0; k < bncols; k++ )
 {
 for( int j = 0; j < ncols; ++j )
 bs[mrows * k + j] = bs[mrows * k + j] / ws[j];
 }
}
 
void DGMSolver::backsolve_polyfit_safeguarded( int dim,
 int degree,
 const bool interp,
 int mrows,
 int ncols,
 double* R,
 int bncols,
 double* bs,
 const double* ws,
 int* degree_out )
{
 if( ncols < 1 ) std::cout << "ERROR: Invalid input to safeguarded polyfit backsolve routine.\n";
#if 0
 std::cout.precision(12);
 std::cout<<"Before backsolve "<<std::endl;
 std::cout<<" V = "<<std::endl;
 for (int k=0; k< ncols; k++){
 for (int j=0; j<mrows; ++j){
 std::cout<<R[mrows*k+j]<<std::endl;
 }
 std::cout<<std::endl;
 }
 std::cout<<std::endl;
 
 //std::cout<<"#pnts = "<<mrows<<std::endl;
 std::cout<<"bs = "<<std::endl;
 std::cout<<std::endl;
 for (int k=0; k< bncols; k++){
 for (int j=0; j<mrows; ++j){
 std::cout<<" "<<bs[mrows*k+j]<<std::endl;
 }
 }
 std::cout<<std::endl;
 
 //std::cout<<" ] "<<std::endl;
 
 std::cout<<"Input ws = [ ";
 for (int k=0; k< ncols; k++){
 std::cout<<ws[k]<<", ";
 }
 std::cout<<" ] "<<std::endl;
 
 std::cout << "R: " << R << "size: [" << mrows << "," << ncols << "]" << std::endl;
 std::cout << "bs: " << bs << "size: [" << mrows << "," << bncols << "]" << std::endl;
 std::cout << "ws: " << ws << "size: [" << ncols << "," << 1 << "]" << std::endl;
 std::cout << "degree_out: " << degree_out << std::endl;
#endif
 
 int deg, numcols = 0;
 
 for( int k = 0; k < bncols; k++ )
 {
 deg = degree;
 /* I think we should consider interp = true/false -Xinglin*/
 if( dim == 1 )
 numcols = deg + 1 - interp;
 else if( dim == 2 )
 numcols = ( deg + 2 ) * ( deg + 1 ) / 2 - interp;
 
 assert( numcols <= ncols );
 
 std::vector< double > bs_bak( numcols );
 
 if( deg >= 2 )
 {
 for( int i = 0; i < numcols; i++ )
 {
 assert( mrows * k + i < mrows * bncols );
 bs_bak.at( i ) = bs[mrows * k + i];
 }
 }
 
 while( deg >= 1 )
 {
 int cend = numcols - 1;
 bool downgrade = false;
 
 // The reconstruction can be applied only on edges (2-d) or faces (3-d)
 assert( cend >= 0 );
 assert( dim > 0 && dim < 3 );
 
 for( int d = deg; d >= 0; d-- )
 {
 int cstart = 0;
 if( dim == 1 )
 {
 cstart = d;
 }
 else if( dim == 2 )
 {
 cstart = ( ( d + 1 ) * d ) / 2;
 // cstart = ((d*(d+1))>>1)-interp;
 }
 
 // Solve for bs
 for( int j = cend; j >= cstart; j-- )
 {
 assert( mrows * k + j < mrows * bncols );
 for( int i = j + 1; i < numcols; ++i )
 {
 assert( mrows * k + i < mrows * bncols );
 assert( mrows * i + j < mrows * ncols ); // check R
 bs[mrows * k + j] = bs[mrows * k + j] - R[mrows * i + j] * bs[mrows * k + i];
 }
 assert( mrows * j + j < mrows * ncols ); // check R
 bs[mrows * k + j] = bs[mrows * k + j] / R[mrows * j + j];
 }
 
 // Checking for change in the coefficient
 if( d >= 2 && d < deg )
 {
 double tol;
 
 if( dim == 1 )
 {
 tol = 1e-06;
 assert( mrows * cstart + cstart < mrows * ncols ); // check R
 double tb = bs_bak.at( cstart ) / R[mrows * cstart + cstart];
 assert( mrows * k + cstart < mrows * bncols );
 if( fabs( bs[mrows * k + cstart] - tb ) > ( 1 + tol ) * fabs( tb ) )
 {
 downgrade = true;
 break;
 }
 }
 
 else if( dim == 2 )
 {
 tol = 0.05;
 
 std::vector< double > tb( cend - cstart + 1 );
 for( int j = 0; j <= ( cend - cstart ); j++ )
 {
 tb.at( j ) = bs_bak.at( cstart + j );
 }
 
 for( int j = cend; j >= cstart; j-- )
 {
 int jind = j - cstart;
 
 for( int i = j + 1; i <= cend; ++i )
 {
 assert( mrows * i + j < mrows * ncols ); // check R
 tb.at( jind ) = tb.at( jind ) - R[mrows * i + j] * tb.at( i - cstart );
 }
 assert( mrows * j + j < mrows * ncols ); // check R
 tb.at( jind ) = tb.at( jind ) / R[mrows * j + j];
 assert( mrows * k + j < mrows * bncols );
 double err = fabs( bs[mrows * k + j] - tb.at( jind ) );
 
 if( ( err > tol ) && ( err >= ( 1 + tol ) * fabs( tb.at( jind ) ) ) )
 {
 downgrade = true;
 break;
 }
 }
 
 if( downgrade ) break;
 }
 }
 
 cend = cstart - 1;
 }
 
 if( !downgrade )
 break;
 else
 {
 deg = deg - 1;
 if( dim == 1 )
 numcols = deg + 1;
 else if( dim == 2 )
 numcols = ( deg + 2 ) * ( deg + 1 ) / 2;
 
 for( int i = 0; i < numcols; i++ )
 {
 assert( mrows * k + i < mrows * bncols );
 bs[mrows * k + i] = bs_bak.at( i );
 }
 }
 }
 assert( k < bncols );
 degree_out[k] = deg;
 
 for( int i = 0; i < numcols; i++ )
 {
 // assert(mrows*k+i < mrows*bncols);
 // assert(i < ncols);
 bs[mrows * k + i] = bs[mrows * k + i] / ws[i];
 }
 
 for( int i = numcols; i < mrows; i++ )
 {
 // assert(mrows*k+i < mrows*bncols);
 bs[mrows * k + i] = 0;
 }
 }
}
 
void DGMSolver::vec_dotprod( const int len, const double* a, const double* b, double* c )
{
 if( !a || !b || !c )
 {
 MB_SET_ERR_RET( "NULL Pointer" );
 }
 for( int i = 0; i < len; ++i )
 {
 c[i] = a[i] * b[i];
 }
}
 
void DGMSolver::vec_scalarprod( const int len, const double* a, const double c, double* b )
{
 if( !a || !b )
 {
 MB_SET_ERR_RET( "NULL Pointer" );
 }
 for( int i = 0; i < len; ++i )
 {
 b[i] = c * a[i];
 }
}
 
void DGMSolver::vec_crossprod( const double a[3], const double b[3], double ( &c )[3] )
{
 c[0] = a[1] * b[2] - a[2] * b[1];
 c[1] = a[2] * b[0] - a[0] * b[2];
 c[2] = a[0] * b[1] - a[1] * b[0];
}
 
double DGMSolver::vec_innerprod( const int len, const double* a, const double* b )
{
 if( !a || !b )
 {
 MB_SET_ERR_RET_VAL( "NULL Pointer", 0.0 );
 }
 double ans = 0;
 for( int i = 0; i < len; ++i )
 {
 ans += a[i] * b[i];
 }
 return ans;
}
 
double DGMSolver::vec_2norm( const int len, const double* a )
{
 if( !a )
 {
 MB_SET_ERR_RET_VAL( "NULL Pointer", 0.0 );
 }
 double w = 0, s = 0;
 for( int k = 0; k < len; k++ )
 w = std::max( w, fabs( a[k] ) );
 
 if( w == 0 )
 {
 return 0;
 }
 else
 {
 for( int k = 0; k < len; k++ )
 {
 s += ( a[k] / w ) * ( a[k] / w );
 }
 s = w * sqrt( s );
 }
 return s;
}
 
double DGMSolver::vec_normalize( const int len, const double* a, double* b )
{
 if( !a || !b )
 {
 MB_SET_ERR_RET_VAL( "NULL Pointer", 0.0 );
 }
 double nrm = 0, mx = 0;
 for( int i = 0; i < len; ++i )
 {
 mx = std::max( fabs( a[i] ), mx );
 }
 if( mx == 0 )
 {
 for( int i = 0; i < len; ++i )
 {
 b[i] = 0;
 }
 return 0;
 }
 for( int i = 0; i < len; ++i )
 {
 nrm += ( a[i] / mx ) * ( a[i] / mx );
 }
 nrm = mx * sqrt( nrm );
 if( nrm == 0 )
 {
 return nrm;
 }
 for( int i = 0; i < len; ++i )
 {
 b[i] = a[i] / nrm;
 }
 return nrm;
}
 
double DGMSolver::vec_distance( const int len, const double* a, const double* b )
{
 double res = 0;
 for( int i = 0; i < len; ++i )
 {
 res += ( a[i] - b[i] ) * ( a[i] - b[i] );
 }
 return sqrt( res );
}
 
void DGMSolver::vec_projoff( const int len, const double* a, const double* b, double* c )
{
 if( !a || !b || !c )
 {
 MB_SET_ERR_RET( "NULL Pointer" );
 }
 // c = a-<a,b>b/<b,b>;
 double bnrm = vec_2norm( len, b );
 if( bnrm == 0 )
 {
 for( int i = 0; i < len; ++i )
 {
 c[i] = a[i];
 }
 return;
 }
 double innerp = vec_innerprod( len, a, b ) / bnrm;
 
 if( innerp == 0 )
 {
 if( c != a )
 {
 for( int i = 0; i < len; ++i )
 {
 c[i] = a[i];
 }
 }
 return;
 }
 
 for( int i = 0; i < len; ++i )
 {
 c[i] = a[i] - innerp * b[i] / bnrm;
 }
}
 
void DGMSolver::vec_linear_operation( const int len,
 const double mu,
 const double* a,
 const double psi,
 const double* b,
 double* c )
{
 if( !a || !b || !c )
 {
 MB_SET_ERR_RET( "NULL Pointer" );
 }
 for( int i = 0; i < len; ++i )
 {
 c[i] = mu * a[i] + psi * b[i];
 }
}
 
void DGMSolver::get_tri_natural_coords( const int dim,
 const double* cornercoords,
 const int npts,
 const double* currcoords,
 double* naturalcoords )
{
 assert( dim == 2 || dim == 3 );
 double a = 0, b = 0, d = 0, tol = 1e-12;
 for( int i = 0; i < dim; ++i )
 {
 a += ( cornercoords[dim + i] - cornercoords[i] ) * ( cornercoords[dim + i] - cornercoords[i] );
 b += ( cornercoords[dim + i] - cornercoords[i] ) * ( cornercoords[2 * dim + i] - cornercoords[i] );
 d += ( cornercoords[2 * dim + i] - cornercoords[i] ) * ( cornercoords[2 * dim + i] - cornercoords[i] );
 }
 double det = a * d - b * b;
 assert( det > 0 );
 for( int ipt = 0; ipt < npts; ++ipt )
 {
 double e = 0, f = 0;
 for( int i = 0; i < dim; ++i )
 {
 e += ( cornercoords[dim + i] - cornercoords[i] ) * ( currcoords[ipt * dim + i] - cornercoords[i] );
 f += ( cornercoords[2 * dim + i] - cornercoords[i] ) * ( currcoords[ipt * dim + i] - cornercoords[i] );
 }
 naturalcoords[ipt * 3 + 1] = ( e * d - b * f ) / det;
 naturalcoords[ipt * 3 + 2] = ( a * f - b * e ) / det;
 naturalcoords[ipt * 3] = 1 - naturalcoords[ipt * 3 + 1] - naturalcoords[ipt * 3 + 2];
 if( naturalcoords[ipt * 3] < -tol || naturalcoords[ipt * 3 + 1] < -tol || naturalcoords[ipt * 3 + 2] < -tol )
 {
 std::cout << "Corners: \n";
 std::cout << cornercoords[0] << "\t" << cornercoords[1] << "\t" << cornercoords[3] << std::endl;
 std::cout << cornercoords[3] << "\t" << cornercoords[4] << "\t" << cornercoords[5] << std::endl;
 std::cout << cornercoords[6] << "\t" << cornercoords[7] << "\t" << cornercoords[8] << std::endl;
 std::cout << "Candidate: \n";
 std::cout << currcoords[ipt * dim] << "\t" << currcoords[ipt * dim + 1] << "\t" << currcoords[ipt * dim + 2]
 << std::endl;
 exit( 0 );
 }
 assert( fabs( naturalcoords[ipt * 3] + naturalcoords[ipt * 3 + 1] + naturalcoords[ipt * 3 + 2] - 1 ) < tol );
 for( int i = 0; i < dim; ++i )
 {
 assert( fabs( naturalcoords[ipt * 3] * cornercoords[i] +
 naturalcoords[ipt * 3 + 1] * cornercoords[dim + i] +
 naturalcoords[ipt * 3 + 2] * cornercoords[2 * dim + i] - currcoords[ipt * dim + i] ) < tol );
 }
 }
}
} // namespace moab