Branch data Line data Source code
1 : : #include "moab/LocalDiscretization/LinearTet.hpp"
2 : : #include "moab/Forward.hpp"
3 : : #include <algorithm>
4 : : #include <math.h>
5 : : #include <limits>
6 : :
7 : : namespace moab
8 : : {
9 : :
10 : : const double LinearTet::corner[4][3] = { { 0, 0, 0 }, { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
11 : :
12 : 14 : ErrorCode LinearTet::initFcn( const double* verts, const int nverts, double*& work )
13 : : {
14 : : // allocate work array as:
15 : : // work[0..8] = T
16 : : // work[9..17] = Tinv
17 : : // work[18] = detT
18 : : // work[19] = detTinv
19 [ + - ][ - + ]: 14 : assert( nverts == 4 && verts );
20 [ + - ][ + - ]: 14 : if( !work ) work = new double[20];
21 : :
22 : 28 : Matrix3 J( verts[1 * 3 + 0] - verts[0 * 3 + 0], verts[2 * 3 + 0] - verts[0 * 3 + 0],
23 : 42 : verts[3 * 3 + 0] - verts[0 * 3 + 0], verts[1 * 3 + 1] - verts[0 * 3 + 1],
24 : 56 : verts[2 * 3 + 1] - verts[0 * 3 + 1], verts[3 * 3 + 1] - verts[0 * 3 + 1],
25 : 56 : verts[1 * 3 + 2] - verts[0 * 3 + 2], verts[2 * 3 + 2] - verts[0 * 3 + 2],
26 [ + - ]: 14 : verts[3 * 3 + 2] - verts[0 * 3 + 2] );
27 [ + - ]: 14 : J.copyto( work );
28 [ + - ][ + - ]: 14 : J.inverse().copyto( work + Matrix3::size );
29 [ + - ]: 14 : work[18] = J.determinant();
30 [ - + ]: 14 : work[19] = ( work[18] < 1e-12 ? std::numeric_limits< double >::max() : 1.0 / work[18] );
31 : :
32 : 14 : return MB_SUCCESS;
33 : : }
34 : :
35 : 142233 : ErrorCode LinearTet::evalFcn( const double* params, const double* field, const int /*ndim*/, const int num_tuples,
36 : : double* /*work*/, double* result )
37 : : {
38 [ + - ][ + - ]: 142233 : assert( params && field && num_tuples > 0 );
[ - + ]
39 [ + - ]: 142233 : std::vector< double > f0( num_tuples );
40 [ + - ]: 142233 : std::copy( field, field + num_tuples, f0.begin() );
41 [ + - ]: 142233 : std::copy( field, field + num_tuples, result );
42 : :
43 [ + + ]: 568932 : for( unsigned i = 1; i < 4; ++i )
44 : : {
45 : 426699 : double p = 0.5 * ( params[i - 1] + 1 ); // transform from -1 <= p <= 1 to 0 <= p <= 1
46 [ + + ]: 1706796 : for( int j = 0; j < num_tuples; j++ )
47 [ + - ]: 1280097 : result[j] += ( field[i * num_tuples + j] - f0[j] ) * p;
48 : : }
49 : :
50 : 142233 : return MB_SUCCESS;
51 : : }
52 : :
53 : 5 : ErrorCode LinearTet::integrateFcn( const double* field, const double* /*verts*/, const int nverts, const int /*ndim*/,
54 : : const int num_tuples, double* work, double* result )
55 : : {
56 [ + - ][ - + ]: 5 : assert( field && num_tuples > 0 );
57 [ + - ]: 5 : std::fill( result, result + num_tuples, 0.0 );
58 [ + + ]: 25 : for( int i = 0; i < nverts; ++i )
59 : : {
60 [ + + ]: 40 : for( int j = 0; j < num_tuples; j++ )
61 : 20 : result[j] += field[i * num_tuples + j];
62 : : }
63 : 5 : double tmp = work[18] / 24.0;
64 [ + + ]: 10 : for( int i = 0; i < num_tuples; i++ )
65 : 5 : result[i] *= tmp;
66 : :
67 : 5 : return MB_SUCCESS;
68 : : }
69 : :
70 : 11110 : ErrorCode LinearTet::jacobianFcn( const double*, const double*, const int, const int, double* work, double* result )
71 : : {
72 : : // jacobian is cached in work array
73 [ - + ]: 11110 : assert( work );
74 : 11110 : std::copy( work, work + 9, result );
75 : 11110 : return MB_SUCCESS;
76 : : }
77 : :
78 : 5555 : ErrorCode LinearTet::reverseEvalFcn( EvalFcn eval, JacobianFcn jacob, InsideFcn ins, const double* posn,
79 : : const double* verts, const int nverts, const int ndim, const double iter_tol,
80 : : const double inside_tol, double* work, double* params, int* is_inside )
81 : : {
82 [ + - ][ - + ]: 5555 : assert( posn && verts );
83 : : return evaluate_reverse( eval, jacob, ins, posn, verts, nverts, ndim, iter_tol, inside_tol, work, params,
84 : 5555 : is_inside );
85 : : }
86 : :
87 : 12210 : int LinearTet::insideFcn( const double* params, const int, const double tol )
88 : : {
89 [ + - ][ + - ]: 12210 : return ( params[0] >= -1.0 - tol && params[1] >= -1.0 - tol && params[2] >= -1.0 - tol &&
[ + - ][ + + ]
90 : 12210 : params[0] + params[1] + params[2] <= 1.0 + tol );
91 : : }
92 : :
93 : 5555 : ErrorCode LinearTet::evaluate_reverse( EvalFcn eval, JacobianFcn jacob, InsideFcn inside_f, const double* posn,
94 : : const double* verts, const int nverts, const int ndim, const double iter_tol,
95 : : const double inside_tol, double* work, double* params, int* inside )
96 : : {
97 : : // TODO: should differentiate between epsilons used for
98 : : // Newton Raphson iteration, and epsilons used for curved boundary geometry errors
99 : : // right now, fix the tolerance used for NR
100 : 5555 : const double error_tol_sqr = iter_tol * iter_tol;
101 : 5555 : CartVect* cvparams = reinterpret_cast< CartVect* >( params );
102 : 5555 : const CartVect* cvposn = reinterpret_cast< const CartVect* >( posn );
103 : :
104 : : // find best initial guess to improve convergence
105 : : CartVect tmp_params[] = { CartVect( -1, -1, -1 ), CartVect( 1, -1, -1 ), CartVect( -1, 1, -1 ),
106 [ + - ][ + - ]: 5555 : CartVect( -1, -1, 1 ) };
[ + - ][ + - ]
107 : 5555 : double resl = std::numeric_limits< double >::max();
108 [ + - ][ + - ]: 5555 : CartVect new_pos, tmp_pos;
109 : : ErrorCode rval;
110 [ + + ]: 27775 : for( unsigned int i = 0; i < 4; i++ )
111 : : {
112 [ + - ][ + - ]: 22220 : rval = ( *eval )( tmp_params[i].array(), verts, ndim, ndim, work, tmp_pos.array() );
[ + - ]
113 [ - + ]: 22220 : if( MB_SUCCESS != rval ) return rval;
114 [ + - ][ + - ]: 22220 : double tmp_resl = ( tmp_pos - *cvposn ).length_squared();
115 [ + + ]: 22220 : if( tmp_resl < resl )
116 : : {
117 : 13427 : *cvparams = tmp_params[i];
118 : 13427 : new_pos = tmp_pos;
119 : 13427 : resl = tmp_resl;
120 : : }
121 : : }
122 : :
123 : : // residual is diff between old and new pos; need to minimize that
124 [ + - ]: 5555 : CartVect res = new_pos - *cvposn;
125 [ + - ]: 5555 : Matrix3 J;
126 [ + - ][ + - ]: 5555 : rval = ( *jacob )( cvparams->array(), verts, nverts, ndim, work, J.array() );
[ + - ]
127 : : #ifndef NDEBUG
128 [ + - ]: 5555 : double det = J.determinant();
129 [ - + ]: 5555 : assert( det > std::numeric_limits< double >::epsilon() );
130 : : #endif
131 [ + - ]: 5555 : Matrix3 Ji = J.inverse();
132 : :
133 : 5555 : int iters = 0;
134 : : // while |res| larger than tol
135 [ - + ]: 5555 : int dum, *tmp_inside = ( inside ? inside : &dum );
136 [ + - ][ + + ]: 120013 : while( res % res > error_tol_sqr )
137 : : {
138 [ - + ]: 114458 : if( ++iters > 25 )
139 : : {
140 : : // if we haven't converged but we're outside, that's defined as success
141 [ # # ]: 0 : *tmp_inside = ( *inside_f )( params, ndim, inside_tol );
142 [ # # ]: 0 : if( !( *tmp_inside ) )
143 : 0 : return MB_SUCCESS;
144 : : else
145 : 0 : return MB_INDEX_OUT_OF_RANGE;
146 : : }
147 : :
148 : : // new params tries to eliminate residual
149 [ + - ][ + - ]: 114458 : *cvparams -= Ji * res;
150 : :
151 : : // get the new forward-evaluated position, and its difference from the target pt
152 [ + - ][ + - ]: 114458 : rval = ( *eval )( params, verts, ndim, ndim, work, new_pos.array() );
153 [ - + ]: 114458 : if( MB_SUCCESS != rval ) return rval;
154 [ + - ]: 114458 : res = new_pos - *cvposn;
155 : : }
156 : :
157 [ + - ][ + - ]: 5555 : if( inside ) *inside = ( *inside_f )( params, ndim, inside_tol );
158 : :
159 : 5555 : return MB_SUCCESS;
160 : : } // Map::evaluate_reverse()
161 : :
162 : 8 : ErrorCode LinearTet::normalFcn( const int ientDim, const int facet, const int nverts, const double* verts,
163 : : double normal[3] )
164 : : {
165 : : // assert(facet < 4 && ientDim == 2 && nverts == 4);
166 [ - + ][ # # ]: 8 : if( nverts != 4 ) MB_SET_ERR( MB_FAILURE, "Incorrect vertex count for passed tet :: expected value = 4 " );
[ # # ][ # # ]
[ # # ][ # # ]
167 [ - + ][ # # ]: 8 : if( ientDim != 2 ) MB_SET_ERR( MB_FAILURE, "Requesting normal for unsupported dimension :: expected value = 2 " );
[ # # ][ # # ]
[ # # ][ # # ]
168 [ + - ][ - + ]: 8 : if( facet > 4 || facet < 0 ) MB_SET_ERR( MB_FAILURE, "Incorrect local face id :: expected value = one of 0-3" );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
169 : :
170 : 8 : int id0 = CN::mConnectivityMap[MBTET][ientDim - 1].conn[facet][0];
171 : 8 : int id1 = CN::mConnectivityMap[MBTET][ientDim - 1].conn[facet][1];
172 : 8 : int id2 = CN::mConnectivityMap[MBTET][ientDim - 1].conn[facet][2];
173 : :
174 : : double x0[3], x1[3];
175 : :
176 [ + + ]: 32 : for( int i = 0; i < 3; i++ )
177 : : {
178 : 24 : x0[i] = verts[3 * id1 + i] - verts[3 * id0 + i];
179 : 24 : x1[i] = verts[3 * id2 + i] - verts[3 * id0 + i];
180 : : }
181 : :
182 : 8 : double a = x0[1] * x1[2] - x1[1] * x0[2];
183 : 8 : double b = x1[0] * x0[2] - x0[0] * x1[2];
184 : 8 : double c = x0[0] * x1[1] - x1[0] * x0[1];
185 : 8 : double nrm = sqrt( a * a + b * b + c * c );
186 : :
187 [ + - ]: 8 : if( nrm > std::numeric_limits< double >::epsilon() )
188 : : {
189 : 8 : normal[0] = a / nrm;
190 : 8 : normal[1] = b / nrm;
191 : 8 : normal[2] = c / nrm;
192 : : }
193 : 8 : return MB_SUCCESS;
194 : : }
195 : :
196 [ + - ][ + - ]: 228 : } // namespace moab
|
