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