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