moab
|
00001 #include "moab/LinearTri.hpp" 00002 #include "moab/Forward.hpp" 00003 #include <algorithm> 00004 00005 namespace moab 00006 { 00007 00008 const double LinearTri::corner[3][2] = { {0,0}, 00009 {1,0}, 00010 {0,1}}; 00011 00012 ErrorCode LinearTri::initFcn(const double *verts, const int /*nverts*/, double *&work) { 00013 // allocate work array as: 00014 // work[0..8] = T 00015 // work[9..17] = Tinv 00016 // work[18] = detT 00017 // work[19] = detTinv 00018 assert(!work && verts); 00019 work = new double[20]; 00020 Matrix3 *T = reinterpret_cast<Matrix3*>(work), 00021 *Tinv = reinterpret_cast<Matrix3*>(work+9); 00022 double *detT = work+18, *detTinv = work+19; 00023 00024 *T = Matrix3(verts[1*3+0]-verts[0*3+0],verts[2*3+0]-verts[0*3+0],0.0, 00025 verts[1*3+1]-verts[0*3+1],verts[2*3+1]-verts[0*3+1],0.0, 00026 verts[1*3+2]-verts[0*3+2],verts[2*3+2]-verts[0*3+2],1.0); 00027 *T *= 0.5; 00028 (*T)(2,2) = 1.0; 00029 00030 *Tinv = T->inverse(); 00031 *detT = T->determinant(); 00032 *detTinv = (0.0 == *detT ? HUGE : 1.0 / *detT); 00033 00034 return MB_SUCCESS; 00035 } 00036 00037 ErrorCode LinearTri::evalFcn(const double *params, const double *field, const int /*ndim*/, const int num_tuples, 00038 double */*work*/, double *result) { 00039 assert(params && field && num_tuples > 0); 00040 // convert to [0,1] 00041 double p1 = 0.5 * (1.0 + params[0]), 00042 p2 = 0.5 * (1.0 + params[1]), 00043 p0 = 1.0 - p1 - p2; 00044 00045 for (int j = 0; j < num_tuples; j++) 00046 result[j] = p0 * field[0*num_tuples+j] + p1 * field[1*num_tuples+j] + p2 * field[2*num_tuples+j]; 00047 00048 return MB_SUCCESS; 00049 } 00050 00051 ErrorCode LinearTri::integrateFcn(const double *field, const double */*verts*/, const int /*nverts*/, const int /*ndim*/, const int num_tuples, 00052 double *work, double *result) 00053 { 00054 assert(field && num_tuples > 0); 00055 double tmp = work[18]; 00056 00057 for (int i = 0; i < num_tuples; i++) 00058 result[i] = tmp * (field[num_tuples+i] + field[2*num_tuples+i]); 00059 00060 return MB_SUCCESS; 00061 } 00062 00063 ErrorCode LinearTri::jacobianFcn(const double *, const double *, const int, const int , 00064 double *work, double *result) 00065 { 00066 // jacobian is cached in work array 00067 assert(work); 00068 std::copy(work, work+9, result); 00069 return MB_SUCCESS; 00070 } 00071 00072 ErrorCode LinearTri::reverseEvalFcn(EvalFcn eval, JacobianFcn jacob, InsideFcn ins, 00073 const double *posn, const double *verts, const int nverts, const int ndim, 00074 const double iter_tol, const double inside_tol, double *work, 00075 double *params, int *is_inside) 00076 { 00077 assert(posn && verts); 00078 return evaluate_reverse(eval, jacob, ins, posn, verts, nverts, ndim, iter_tol, inside_tol, work, 00079 params, is_inside); 00080 } 00081 00082 int LinearTri::insideFcn(const double *params, const int , const double tol) 00083 { 00084 return (params[0] >= -1.0-tol && params[1] >= -1.0-tol && 00085 params[0] + params[1] <= 1.0+tol); 00086 00087 } 00088 00089 ErrorCode LinearTri::evaluate_reverse(EvalFcn eval, JacobianFcn jacob, InsideFcn inside_f, 00090 const double *posn, const double *verts, const int nverts, 00091 const int ndim, const double iter_tol, const double inside_tol, 00092 double *work, double *params, int *inside) { 00093 // TODO: should differentiate between epsilons used for 00094 // Newton Raphson iteration, and epsilons used for curved boundary geometry errors 00095 // right now, fix the tolerance used for NR 00096 const double error_tol_sqr = iter_tol*iter_tol; 00097 CartVect *cvparams = reinterpret_cast<CartVect*>(params); 00098 const CartVect *cvposn = reinterpret_cast<const CartVect*>(posn); 00099 00100 // find best initial guess to improve convergence 00101 CartVect tmp_params[] = {CartVect(-1,-1,-1), CartVect(1,-1,-1), CartVect(-1,1,-1)}; 00102 double resl = HUGE; 00103 CartVect new_pos, tmp_pos; 00104 ErrorCode rval; 00105 for (unsigned int i = 0; i < 3; i++) { 00106 rval = (*eval)(tmp_params[i].array(), verts, ndim, 3, work, tmp_pos.array()); 00107 if (MB_SUCCESS != rval) return rval; 00108 double tmp_resl = (tmp_pos-*cvposn).length_squared(); 00109 if (tmp_resl < resl) { 00110 *cvparams = tmp_params[i]; 00111 new_pos = tmp_pos; 00112 resl = tmp_resl; 00113 } 00114 } 00115 00116 // residual is diff between old and new pos; need to minimize that 00117 CartVect res = new_pos - *cvposn; 00118 Matrix3 J; 00119 rval = (*jacob)(cvparams->array(), verts, nverts, ndim, work, J[0]); 00120 double det = J.determinant(); 00121 assert(det > std::numeric_limits<double>::epsilon()); 00122 Matrix3 Ji = J.inverse(1.0/det); 00123 00124 int iters=0; 00125 // while |res| larger than tol 00126 while (res % res > error_tol_sqr) { 00127 if(++iters>25) 00128 return MB_FAILURE; 00129 00130 // new params tries to eliminate residual 00131 *cvparams -= Ji * res; 00132 00133 // get the new forward-evaluated position, and its difference from the target pt 00134 rval = (*eval)(params, verts, ndim, 3, work, new_pos.array()); 00135 if (MB_SUCCESS != rval) return rval; 00136 res = new_pos - *cvposn; 00137 } 00138 00139 if (inside) 00140 *inside = (*inside_f)(params, ndim, inside_tol); 00141 00142 return MB_SUCCESS; 00143 }// Map::evaluate_reverse() 00144 00145 } // namespace moab