MOAB: Mesh Oriented datABase
(version 5.3.1)
|
00001 /* ***************************************************************** 00002 MESQUITE -- The Mesh Quality Improvement Toolkit 00003 00004 Copyright 2004 Sandia Corporation and Argonne National 00005 Laboratory. Under the terms of Contract DE-AC04-94AL85000 00006 with Sandia Corporation, the U.S. Government retains certain 00007 rights in this software. 00008 00009 This library is free software; you can redistribute it and/or 00010 modify it under the terms of the GNU Lesser General Public 00011 License as published by the Free Software Foundation; either 00012 version 2.1 of the License, or (at your option) any later version. 00013 00014 This library is distributed in the hope that it will be useful, 00015 but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00017 Lesser General Public License for more details. 00018 00019 You should have received a copy of the GNU Lesser General Public License 00020 (lgpl.txt) along with this library; if not, write to the Free Software 00021 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00022 00023 diachin2@llnl.gov, djmelan@sandia.gov, mbrewer@sandia.gov, 00024 pknupp@sandia.gov, tleurent@mcs.anl.gov, tmunson@mcs.anl.gov 00025 00026 ***************************************************************** */ 00027 // -*- Mode : c++; tab-width: 3; c-tab-always-indent: t; indent-tabs-mode: nil; c-basic-offset: 3 00028 // -*- 00029 00030 /*! \file MeanRatioFunctions.hpp 00031 00032 Header that defines generalized Mean Ratio function, gradient, and hessian 00033 evaluations. 00034 00035 \author Todd Munson 00036 \date 2003-06-11 00037 */ 00038 00039 #ifndef MeanRatioFunctions_hpp 00040 #define MeanRatioFunctions_hpp 00041 00042 #include <cmath> 00043 #include "Mesquite.hpp" 00044 #include "Vector3D.hpp" 00045 #include "Matrix3D.hpp" 00046 #include "Exponent.hpp" 00047 00048 namespace MBMesquite 00049 { 00050 /***************************************************************************/ 00051 /* Gradient calculation courtesy of Paul Hovland. The code was modified */ 00052 /* to reduce the number of flops and intermediate variables, and improve */ 00053 /* the locality of reference. */ 00054 /***************************************************************************/ 00055 /* The Hessian calculation is computes everyting in (x,y,z) blocks and */ 00056 /* stores only the upper triangular blocks. The results are put into */ 00057 /* (c1,c2,c3,c4) order prior to returning the Hessian matrix. */ 00058 /***************************************************************************/ 00059 /* The form of the function, gradient, and Hessian follows: */ 00060 /* o(x) = a * pow(f(A(x)), b) * pow(g(A(x)), c) */ 00061 /* where A(x) is the incidence matrix generated by: */ 00062 /* [x1-x0 x2-x0 x3-x0] */ 00063 /* A(x) = [y1-y0 y2-y0 y3-y0] * inv(W) */ 00064 /* [z1-z0 z2-z0 z3-z0] */ 00065 /* and f() is the squared Frobenius norm of A(x), and g() is the */ 00066 /* determinant of A(x). */ 00067 /* */ 00068 /* The gradient is calculated as follows: */ 00069 /* alpha := a*b*pow(f(A(x)),b-1)*pow(g(A(x)),c) */ 00070 /* beta := a*c*pow(f(A(x)),b)*pow(g(A(x)),c-1) */ 00071 /* */ 00072 /* do/dx = (alpha * (df/dA) + beta * (dg/dA)) (dA/dx) */ 00073 /* */ 00074 /* (df/dA)_i = 2*A_i */ 00075 /* (dg/dA)_i = A_j*A_k - A_l*A_m for some {j,k,l,m} */ 00076 /* */ 00077 /* d^2o/dx^2 = (dA/dx)' * ((d alpha/dA) * (df/dA) + */ 00078 /* (d beta/dA) * (dg/dA) */ 00079 /* alpha * (d^2f/dA^2) */ 00080 /* beta * (d^2g/dA^2)) * (dA/dx) */ 00081 /* */ 00082 /* Note: since A(x) is a linear function, there are no terms involving */ 00083 /* d^2A/dx^2 since this matrix is zero. */ 00084 /* */ 00085 /* gamma := a*b*c*pow(f(A(x)),b-1)*pow(g(A(x)),c-1) */ 00086 /* delta := a*c*(c-1)*pow(f(A(x)),b)*pow(g(A(x)),c-2) */ 00087 /* psi := a*b*(b-1)*pow(f(A(x)),b-2)*pow(g(A(x)),c) */ 00088 /* */ 00089 /* d^2o/dx^2 = (dA/dx)' * (gamma*((dg/dA)'*(df/dA) + (df/dA)'*(dg/dA)) + */ 00090 /* delta* (dg/dA)'*(dg/dA) + */ 00091 /* psi* (df/dA)'*(df/dA) + */ 00092 /* alpha*(d^2f/dA^2) + */ 00093 /* beta*(d^2g/dA^2)) * (dA/dx) */ 00094 /* */ 00095 /* Note: (df/dA) and (dg/dA) are row vectors and we only calculate the */ 00096 /* upper triangular part of the inner matrices. */ 00097 /* */ 00098 /* For regular tetrahedral elements, we have the following: */ 00099 /* */ 00100 /* [-1 1 0 0 ] */ 00101 /* M = [-sqrt(3) -sqrt(3) 2*sqrt(3) 0 ] */ 00102 /* [-sqrt(6) -sqrt(6) -sqrt(6) 3*sqrt(6) ] */ 00103 /* */ 00104 /* [M 0 0] */ 00105 /* dA/dx = [0 M 0] */ 00106 /* [0 0 M] */ 00107 /***************************************************************************/ 00108 00109 /*****************************************************************************/ 00110 /* Not all compilers substitute out constants (especially the square root). */ 00111 /* Therefore, they are substituted out manually. The values below were */ 00112 /* calculated on a solaris machine using long doubles. I believe they are */ 00113 /* accurate. */ 00114 /*****************************************************************************/ 00115 00116 #define isqrt3 5.77350269189625797959429519858e-01 /* 1.0/sqrt(3.0)*/ 00117 #define tisqrt3 1.15470053837925159591885903972e+00 /* 2.0/sqrt(3.0)*/ 00118 #define isqrt6 4.08248290463863052509822647505e-01 /* 1.0/sqrt(6.0)*/ 00119 #define tisqrt6 1.22474487139158915752946794252e+00 /* 3.0/sqrt(6.0)*/ 00120 00121 /*****************************************************************************/ 00122 /* The following set of functions reference triangular elements to an */ 00123 /* equilateral triangle in the plane defined by the normal. They are */ 00124 /* used when assessing the quality of a triangular element. A zero */ 00125 /* return value indicates success, while a nonzero value indicates failure. */ 00126 /*****************************************************************************/ 00127 00128 /*****************************************************************************/ 00129 /* Function evaluation requires 44 flops. */ 00130 /* Reductions possible when b == 1 or c == 1 */ 00131 /*****************************************************************************/ 00132 inline bool m_fcn_2e( double& obj, const Vector3D x[3], const Vector3D& n, const double a, const Exponent& b, 00133 const Exponent& c ) 00134 { 00135 double matr[9], f; 00136 double g; 00137 00138 /* Calculate M = [A*inv(W) n] */ 00139 matr[0] = x[1][0] - x[0][0]; 00140 matr[1] = ( 2.0 * x[2][0] - x[1][0] - x[0][0] ) * isqrt3; 00141 matr[2] = n[0]; 00142 00143 matr[3] = x[1][1] - x[0][1]; 00144 matr[4] = ( 2.0 * x[2][1] - x[1][1] - x[0][1] ) * isqrt3; 00145 matr[5] = n[1]; 00146 00147 matr[6] = x[1][2] - x[0][2]; 00148 matr[7] = ( 2.0 * x[2][2] - x[1][2] - x[0][2] ) * isqrt3; 00149 matr[8] = n[2]; 00150 00151 /* Calculate det(M). */ 00152 g = matr[0] * ( matr[4] * matr[8] - matr[5] * matr[7] ) + matr[3] * ( matr[2] * matr[7] - matr[1] * matr[8] ) + 00153 matr[6] * ( matr[1] * matr[5] - matr[2] * matr[4] ); 00154 if( g < MSQ_MIN ) 00155 { 00156 obj = g; 00157 return false; 00158 } 00159 00160 /* Calculate norm(M). */ 00161 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[3] * matr[3] + matr[4] * matr[4] + matr[6] * matr[6] + 00162 matr[7] * matr[7]; 00163 00164 /* Calculate objective function. */ 00165 obj = a * b.raise( f ) * c.raise( g ); 00166 return true; 00167 } 00168 00169 /*****************************************************************************/ 00170 /* Gradient evaluation requires 88 flops. */ 00171 /* Reductions possible when b == 1 or c == 1 */ 00172 /*****************************************************************************/ 00173 inline bool g_fcn_2e( double& obj, Vector3D g_obj[3], const Vector3D x[3], const Vector3D& n, const double a, 00174 const Exponent& b, const Exponent& c ) 00175 { 00176 double matr[9], f; 00177 double adj_m[9], g; // adj_m[2,5,8] not used 00178 double loc1, loc2, loc3; 00179 00180 /* Calculate M = [A*inv(W) n] */ 00181 matr[0] = x[1][0] - x[0][0]; 00182 matr[1] = ( 2.0 * x[2][0] - x[1][0] - x[0][0] ) * isqrt3; 00183 matr[2] = n[0]; 00184 00185 matr[3] = x[1][1] - x[0][1]; 00186 matr[4] = ( 2.0 * x[2][1] - x[1][1] - x[0][1] ) * isqrt3; 00187 matr[5] = n[1]; 00188 00189 matr[6] = x[1][2] - x[0][2]; 00190 matr[7] = ( 2.0 * x[2][2] - x[1][2] - x[0][2] ) * isqrt3; 00191 matr[8] = n[2]; 00192 00193 /* Calculate det([n M]). */ 00194 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 00195 loc2 = matr[2] * matr[7] - matr[1] * matr[8]; 00196 loc3 = matr[1] * matr[5] - matr[2] * matr[4]; 00197 g = matr[0] * loc1 + matr[3] * loc2 + matr[6] * loc3; 00198 if( g < MSQ_MIN ) 00199 { 00200 obj = g; 00201 return false; 00202 } 00203 00204 /* Calculate norm(M). */ 00205 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[3] * matr[3] + matr[4] * matr[4] + matr[6] * matr[6] + 00206 matr[7] * matr[7]; 00207 00208 /* Calculate objective function. */ 00209 obj = a * b.raise( f ) * c.raise( g ); 00210 00211 /* Calculate the derivative of the objective function. */ 00212 f = b.value() * obj / f; /* Constant on nabla f */ 00213 g = c.value() * obj / g; /* Constant on nable g */ 00214 f *= 2.0; /* Modification for nabla f */ 00215 00216 adj_m[0] = matr[0] * f + loc1 * g; 00217 adj_m[3] = matr[3] * f + loc2 * g; 00218 adj_m[6] = matr[6] * f + loc3 * g; 00219 00220 loc1 = matr[0] * g; 00221 loc2 = matr[3] * g; 00222 loc3 = matr[6] * g; 00223 00224 adj_m[1] = matr[1] * f + loc3 * matr[5] - loc2 * matr[8]; 00225 adj_m[4] = matr[4] * f + loc1 * matr[8] - loc3 * matr[2]; 00226 adj_m[7] = matr[7] * f + loc2 * matr[2] - loc1 * matr[5]; 00227 00228 loc1 = isqrt3 * adj_m[1]; 00229 g_obj[0][0] = -adj_m[0] - loc1; 00230 g_obj[1][0] = adj_m[0] - loc1; 00231 g_obj[2][0] = 2.0 * loc1; 00232 00233 loc1 = isqrt3 * adj_m[4]; 00234 g_obj[0][1] = -adj_m[3] - loc1; 00235 g_obj[1][1] = adj_m[3] - loc1; 00236 g_obj[2][1] = 2.0 * loc1; 00237 00238 loc1 = isqrt3 * adj_m[7]; 00239 g_obj[0][2] = -adj_m[6] - loc1; 00240 g_obj[1][2] = adj_m[6] - loc1; 00241 g_obj[2][2] = 2.0 * loc1; 00242 return true; 00243 } 00244 00245 /*****************************************************************************/ 00246 /* Hessian evaluation requires 316 flops. */ 00247 /* Reductions possible when b == 1 or c == 1 */ 00248 /*****************************************************************************/ 00249 inline bool h_fcn_2e( double& obj, Vector3D g_obj[3], Matrix3D h_obj[6], const Vector3D x[3], const Vector3D& n, 00250 const double a, const Exponent& b, const Exponent& c ) 00251 { 00252 double matr[9], f; 00253 double adj_m[9], g; // adj_m[2,5,8] not used 00254 double dg[9]; // dg[2,5,8] not used 00255 double loc0, loc1, loc2, loc3, loc4; 00256 double A[12], J_A[6], J_B[9], J_C[9], cross; // only 2x2 corners used 00257 00258 /* Calculate M = [A*inv(W) n] */ 00259 matr[0] = x[1][0] - x[0][0]; 00260 matr[1] = ( 2.0 * x[2][0] - x[1][0] - x[0][0] ) * isqrt3; 00261 matr[2] = n[0]; 00262 00263 matr[3] = x[1][1] - x[0][1]; 00264 matr[4] = ( 2.0 * x[2][1] - x[1][1] - x[0][1] ) * isqrt3; 00265 matr[5] = n[1]; 00266 00267 matr[6] = x[1][2] - x[0][2]; 00268 matr[7] = ( 2.0 * x[2][2] - x[1][2] - x[0][2] ) * isqrt3; 00269 matr[8] = n[2]; 00270 00271 /* Calculate det([n M]). */ 00272 dg[0] = matr[4] * matr[8] - matr[5] * matr[7]; 00273 dg[3] = matr[2] * matr[7] - matr[1] * matr[8]; 00274 dg[6] = matr[1] * matr[5] - matr[2] * matr[4]; 00275 g = matr[0] * dg[0] + matr[3] * dg[3] + matr[6] * dg[6]; 00276 if( g < MSQ_MIN ) 00277 { 00278 obj = g; 00279 return false; 00280 } 00281 00282 /* Calculate norm(M). */ 00283 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[3] * matr[3] + matr[4] * matr[4] + matr[6] * matr[6] + 00284 matr[7] * matr[7]; 00285 00286 loc3 = f; 00287 loc4 = g; 00288 00289 /* Calculate objective function. */ 00290 obj = a * b.raise( f ) * c.raise( g ); 00291 00292 /* Calculate the derivative of the objective function. */ 00293 f = b.value() * obj / f; /* Constant on nabla f */ 00294 g = c.value() * obj / g; /* Constant on nable g */ 00295 f *= 2.0; /* Modification for nabla f */ 00296 00297 dg[1] = matr[5] * matr[6] - matr[3] * matr[8]; 00298 dg[4] = matr[0] * matr[8] - matr[2] * matr[6]; 00299 dg[7] = matr[2] * matr[3] - matr[0] * matr[5]; 00300 00301 adj_m[0] = matr[0] * f + dg[0] * g; 00302 adj_m[1] = matr[1] * f + dg[1] * g; 00303 adj_m[3] = matr[3] * f + dg[3] * g; 00304 adj_m[4] = matr[4] * f + dg[4] * g; 00305 adj_m[6] = matr[6] * f + dg[6] * g; 00306 adj_m[7] = matr[7] * f + dg[7] * g; 00307 00308 loc1 = isqrt3 * adj_m[1]; 00309 g_obj[0][0] = -adj_m[0] - loc1; 00310 g_obj[1][0] = adj_m[0] - loc1; 00311 g_obj[2][0] = 2.0 * loc1; 00312 00313 loc1 = isqrt3 * adj_m[4]; 00314 g_obj[0][1] = -adj_m[3] - loc1; 00315 g_obj[1][1] = adj_m[3] - loc1; 00316 g_obj[2][1] = 2.0 * loc1; 00317 00318 loc1 = isqrt3 * adj_m[7]; 00319 g_obj[0][2] = -adj_m[6] - loc1; 00320 g_obj[1][2] = adj_m[6] - loc1; 00321 g_obj[2][2] = 2.0 * loc1; 00322 00323 /* Calculate the hessian of the objective. */ 00324 loc0 = f; /* Constant on nabla^2 f */ 00325 loc1 = g; /* Constant on nabla^2 g */ 00326 cross = f * c.value() / loc4; /* Constant on nabla g nabla f */ 00327 f = f * ( b.value() - 1 ) / loc3; /* Constant on nabla f nabla f */ 00328 g = g * ( c.value() - 1 ) / loc4; /* Constant on nabla g nabla g */ 00329 f *= 2.0; /* Modification for nabla f */ 00330 00331 /* First block of rows */ 00332 loc3 = matr[0] * f + dg[0] * cross; 00333 loc4 = dg[0] * g + matr[0] * cross; 00334 00335 J_A[0] = loc0 + loc3 * matr[0] + loc4 * dg[0]; 00336 J_A[1] = loc3 * matr[1] + loc4 * dg[1]; 00337 J_B[0] = loc3 * matr[3] + loc4 * dg[3]; 00338 J_B[1] = loc3 * matr[4] + loc4 * dg[4]; 00339 J_C[0] = loc3 * matr[6] + loc4 * dg[6]; 00340 J_C[1] = loc3 * matr[7] + loc4 * dg[7]; 00341 00342 loc3 = matr[1] * f + dg[1] * cross; 00343 loc4 = dg[1] * g + matr[1] * cross; 00344 00345 J_A[3] = loc0 + loc3 * matr[1] + loc4 * dg[1]; 00346 J_B[3] = loc3 * matr[3] + loc4 * dg[3]; 00347 J_B[4] = loc3 * matr[4] + loc4 * dg[4]; 00348 J_C[3] = loc3 * matr[6] + loc4 * dg[6]; 00349 J_C[4] = loc3 * matr[7] + loc4 * dg[7]; 00350 00351 /* First diagonal block */ 00352 loc2 = isqrt3 * J_A[1]; 00353 A[0] = -J_A[0] - loc2; 00354 A[1] = J_A[0] - loc2; 00355 00356 loc2 = isqrt3 * J_A[3]; 00357 A[4] = -J_A[1] - loc2; 00358 A[5] = J_A[1] - loc2; 00359 A[6] = 2.0 * loc2; 00360 00361 loc2 = isqrt3 * A[4]; 00362 h_obj[0][0][0] = -A[0] - loc2; 00363 h_obj[1][0][0] = A[0] - loc2; 00364 h_obj[2][0][0] = 2.0 * loc2; 00365 00366 loc2 = isqrt3 * A[5]; 00367 h_obj[3][0][0] = A[1] - loc2; 00368 h_obj[4][0][0] = 2.0 * loc2; 00369 00370 h_obj[5][0][0] = tisqrt3 * A[6]; 00371 00372 /* First off-diagonal block */ 00373 loc2 = matr[8] * loc1; 00374 J_B[1] += loc2; 00375 J_B[3] -= loc2; 00376 00377 loc2 = isqrt3 * J_B[3]; 00378 A[0] = -J_B[0] - loc2; 00379 A[1] = J_B[0] - loc2; 00380 A[2] = 2.0 * loc2; 00381 00382 loc2 = isqrt3 * J_B[4]; 00383 A[4] = -J_B[1] - loc2; 00384 A[5] = J_B[1] - loc2; 00385 A[6] = 2.0 * loc2; 00386 00387 loc2 = isqrt3 * A[4]; 00388 h_obj[0][0][1] = -A[0] - loc2; 00389 h_obj[1][0][1] = A[0] - loc2; 00390 h_obj[2][0][1] = 2.0 * loc2; 00391 00392 loc2 = isqrt3 * A[5]; 00393 h_obj[1][1][0] = -A[1] - loc2; 00394 h_obj[3][0][1] = A[1] - loc2; 00395 h_obj[4][0][1] = 2.0 * loc2; 00396 00397 loc2 = isqrt3 * A[6]; 00398 h_obj[2][1][0] = -A[2] - loc2; 00399 h_obj[4][1][0] = A[2] - loc2; 00400 h_obj[5][0][1] = 2.0 * loc2; 00401 00402 /* Second off-diagonal block */ 00403 loc2 = matr[5] * loc1; 00404 J_C[1] -= loc2; 00405 J_C[3] += loc2; 00406 00407 loc2 = isqrt3 * J_C[3]; 00408 A[0] = -J_C[0] - loc2; 00409 A[1] = J_C[0] - loc2; 00410 A[2] = 2.0 * loc2; 00411 00412 loc2 = isqrt3 * J_C[4]; 00413 A[4] = -J_C[1] - loc2; 00414 A[5] = J_C[1] - loc2; 00415 A[6] = 2.0 * loc2; 00416 00417 loc2 = isqrt3 * A[4]; 00418 h_obj[0][0][2] = -A[0] - loc2; 00419 h_obj[1][0][2] = A[0] - loc2; 00420 h_obj[2][0][2] = 2.0 * loc2; 00421 00422 loc2 = isqrt3 * A[5]; 00423 h_obj[1][2][0] = -A[1] - loc2; 00424 h_obj[3][0][2] = A[1] - loc2; 00425 h_obj[4][0][2] = 2.0 * loc2; 00426 00427 loc2 = isqrt3 * A[6]; 00428 h_obj[2][2][0] = -A[2] - loc2; 00429 h_obj[4][2][0] = A[2] - loc2; 00430 h_obj[5][0][2] = 2.0 * loc2; 00431 00432 /* Second block of rows */ 00433 loc3 = matr[3] * f + dg[3] * cross; 00434 loc4 = dg[3] * g + matr[3] * cross; 00435 00436 J_A[0] = loc0 + loc3 * matr[3] + loc4 * dg[3]; 00437 J_A[1] = loc3 * matr[4] + loc4 * dg[4]; 00438 J_B[0] = loc3 * matr[6] + loc4 * dg[6]; 00439 J_B[1] = loc3 * matr[7] + loc4 * dg[7]; 00440 00441 loc3 = matr[4] * f + dg[4] * cross; 00442 loc4 = dg[4] * g + matr[4] * cross; 00443 00444 J_A[3] = loc0 + loc3 * matr[4] + loc4 * dg[4]; 00445 J_B[3] = loc3 * matr[6] + loc4 * dg[6]; 00446 J_B[4] = loc3 * matr[7] + loc4 * dg[7]; 00447 00448 /* Second diagonal block */ 00449 loc2 = isqrt3 * J_A[1]; 00450 A[0] = -J_A[0] - loc2; 00451 A[1] = J_A[0] - loc2; 00452 00453 loc2 = isqrt3 * J_A[3]; 00454 A[4] = -J_A[1] - loc2; 00455 A[5] = J_A[1] - loc2; 00456 A[6] = 2.0 * loc2; 00457 00458 loc2 = isqrt3 * A[4]; 00459 h_obj[0][1][1] = -A[0] - loc2; 00460 h_obj[1][1][1] = A[0] - loc2; 00461 h_obj[2][1][1] = 2.0 * loc2; 00462 00463 loc2 = isqrt3 * A[5]; 00464 h_obj[3][1][1] = A[1] - loc2; 00465 h_obj[4][1][1] = 2.0 * loc2; 00466 00467 h_obj[5][1][1] = tisqrt3 * A[6]; 00468 00469 /* Third off-diagonal block */ 00470 loc2 = matr[2] * loc1; 00471 J_B[1] += loc2; 00472 J_B[3] -= loc2; 00473 00474 loc2 = isqrt3 * J_B[3]; 00475 A[0] = -J_B[0] - loc2; 00476 A[1] = J_B[0] - loc2; 00477 A[2] = 2.0 * loc2; 00478 00479 loc2 = isqrt3 * J_B[4]; 00480 A[4] = -J_B[1] - loc2; 00481 A[5] = J_B[1] - loc2; 00482 A[6] = 2.0 * loc2; 00483 00484 loc2 = isqrt3 * A[4]; 00485 h_obj[0][1][2] = -A[0] - loc2; 00486 h_obj[1][1][2] = A[0] - loc2; 00487 h_obj[2][1][2] = 2.0 * loc2; 00488 00489 loc2 = isqrt3 * A[5]; 00490 h_obj[1][2][1] = -A[1] - loc2; 00491 h_obj[3][1][2] = A[1] - loc2; 00492 h_obj[4][1][2] = 2.0 * loc2; 00493 00494 loc2 = isqrt3 * A[6]; 00495 h_obj[2][2][1] = -A[2] - loc2; 00496 h_obj[4][2][1] = A[2] - loc2; 00497 h_obj[5][1][2] = 2.0 * loc2; 00498 00499 /* Third block of rows */ 00500 loc3 = matr[6] * f + dg[6] * cross; 00501 loc4 = dg[6] * g + matr[6] * cross; 00502 00503 J_A[0] = loc0 + loc3 * matr[6] + loc4 * dg[6]; 00504 J_A[1] = loc3 * matr[7] + loc4 * dg[7]; 00505 00506 loc3 = matr[7] * f + dg[7] * cross; 00507 loc4 = dg[7] * g + matr[7] * cross; 00508 00509 J_A[3] = loc0 + loc3 * matr[7] + loc4 * dg[7]; 00510 00511 /* Third diagonal block */ 00512 loc2 = isqrt3 * J_A[1]; 00513 A[0] = -J_A[0] - loc2; 00514 A[1] = J_A[0] - loc2; 00515 00516 loc2 = isqrt3 * J_A[3]; 00517 A[4] = -J_A[1] - loc2; 00518 A[5] = J_A[1] - loc2; 00519 A[6] = 2.0 * loc2; 00520 00521 loc2 = isqrt3 * A[4]; 00522 h_obj[0][2][2] = -A[0] - loc2; 00523 h_obj[1][2][2] = A[0] - loc2; 00524 h_obj[2][2][2] = 2.0 * loc2; 00525 00526 loc2 = isqrt3 * A[5]; 00527 h_obj[3][2][2] = A[1] - loc2; 00528 h_obj[4][2][2] = 2.0 * loc2; 00529 00530 h_obj[5][2][2] = tisqrt3 * A[6]; 00531 00532 // completes diagonal blocks. 00533 h_obj[0].fill_lower_triangle(); 00534 h_obj[3].fill_lower_triangle(); 00535 h_obj[5].fill_lower_triangle(); 00536 return true; 00537 } 00538 00539 /*****************************************************************************/ 00540 /* The following set of functions reference triangular elements to an */ 00541 /* right triangle in the plane defined by the normal. They are used when */ 00542 /* assessing the quality of a quadrilateral elements. A zero return value */ 00543 /* indicates success, while a nonzero value indicates failure. */ 00544 /*****************************************************************************/ 00545 00546 /*****************************************************************************/ 00547 /* Function evaluation -- requires 41 flops. */ 00548 /* Reductions possible when b == 1, c == 1, or d == 1 */ 00549 /*****************************************************************************/ 00550 inline bool m_fcn_2i( double& obj, const Vector3D x[3], const Vector3D& n, const double a, const Exponent& b, 00551 const Exponent& c, const Vector3D& d ) 00552 { 00553 double matr[9]; 00554 double f; 00555 double g; 00556 00557 /* Calculate M = A*inv(W). */ 00558 matr[0] = d[0] * ( x[1][0] - x[0][0] ); 00559 matr[1] = d[1] * ( x[2][0] - x[0][0] ); 00560 matr[2] = n[0]; 00561 00562 matr[3] = d[0] * ( x[1][1] - x[0][1] ); 00563 matr[4] = d[1] * ( x[2][1] - x[0][1] ); 00564 matr[5] = n[1]; 00565 00566 matr[6] = d[0] * ( x[1][2] - x[0][2] ); 00567 matr[7] = d[1] * ( x[2][2] - x[0][2] ); 00568 matr[8] = n[2]; 00569 00570 /* Calculate det(M). */ 00571 g = matr[0] * ( matr[4] * matr[8] - matr[5] * matr[7] ) + matr[3] * ( matr[2] * matr[7] - matr[1] * matr[8] ) + 00572 matr[6] * ( matr[1] * matr[5] - matr[2] * matr[4] ); 00573 if( g < MSQ_MIN ) 00574 { 00575 obj = g; 00576 return false; 00577 } 00578 00579 /* Calculate norm(M). */ 00580 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[3] * matr[3] + matr[4] * matr[4] + matr[6] * matr[6] + 00581 matr[7] * matr[7]; 00582 00583 /* Calculate objective function. */ 00584 obj = a * b.raise( f ) * c.raise( g ); 00585 return true; 00586 } 00587 00588 /*****************************************************************************/ 00589 /* Gradient requires 82 flops. */ 00590 /* Reductions possible when b == 1, c == 1, or d == 1 */ 00591 /*****************************************************************************/ 00592 inline bool g_fcn_2i( double& obj, Vector3D g_obj[3], const Vector3D x[3], const Vector3D& n, const double a, 00593 const Exponent& b, const Exponent& c, const Vector3D& d ) 00594 { 00595 double matr[9], f; 00596 double adj_m[9], g; // adj_m[2,5,8] not used 00597 double loc1, loc2, loc3; 00598 00599 /* Calculate M = [A*inv(W) n] */ 00600 matr[0] = d[0] * ( x[1][0] - x[0][0] ); 00601 matr[1] = d[1] * ( x[2][0] - x[0][0] ); 00602 matr[2] = n[0]; 00603 00604 matr[3] = d[0] * ( x[1][1] - x[0][1] ); 00605 matr[4] = d[1] * ( x[2][1] - x[0][1] ); 00606 matr[5] = n[1]; 00607 00608 matr[6] = d[0] * ( x[1][2] - x[0][2] ); 00609 matr[7] = d[1] * ( x[2][2] - x[0][2] ); 00610 matr[8] = n[2]; 00611 00612 /* Calculate det([n M]). */ 00613 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 00614 loc2 = matr[2] * matr[7] - matr[1] * matr[8]; 00615 loc3 = matr[1] * matr[5] - matr[2] * matr[4]; 00616 g = matr[0] * loc1 + matr[3] * loc2 + matr[6] * loc3; 00617 if( g < MSQ_MIN ) 00618 { 00619 obj = g; 00620 return false; 00621 } 00622 00623 /* Calculate norm(M). */ 00624 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[3] * matr[3] + matr[4] * matr[4] + matr[6] * matr[6] + 00625 matr[7] * matr[7]; 00626 00627 /* Calculate objective function. */ 00628 obj = a * b.raise( f ) * c.raise( g ); 00629 00630 /* Calculate the derivative of the objective function. */ 00631 f = b.value() * obj / f; /* Constant on nabla f */ 00632 g = c.value() * obj / g; /* Constant on nable g */ 00633 f *= 2.0; /* Modification for nabla f */ 00634 00635 adj_m[0] = d[0] * ( matr[0] * f + loc1 * g ); 00636 adj_m[3] = d[0] * ( matr[3] * f + loc2 * g ); 00637 adj_m[6] = d[0] * ( matr[6] * f + loc3 * g ); 00638 00639 loc1 = matr[0] * g; 00640 loc2 = matr[3] * g; 00641 loc3 = matr[6] * g; 00642 00643 adj_m[1] = d[1] * ( matr[1] * f + loc3 * matr[5] - loc2 * matr[8] ); 00644 adj_m[4] = d[1] * ( matr[4] * f + loc1 * matr[8] - loc3 * matr[2] ); 00645 adj_m[7] = d[1] * ( matr[7] * f + loc2 * matr[2] - loc1 * matr[5] ); 00646 00647 g_obj[0][0] = -adj_m[0] - adj_m[1]; 00648 g_obj[1][0] = adj_m[0]; 00649 g_obj[2][0] = adj_m[1]; 00650 00651 g_obj[0][1] = -adj_m[3] - adj_m[4]; 00652 g_obj[1][1] = adj_m[3]; 00653 g_obj[2][1] = adj_m[4]; 00654 00655 g_obj[0][2] = -adj_m[6] - adj_m[7]; 00656 g_obj[1][2] = adj_m[6]; 00657 g_obj[2][2] = adj_m[7]; 00658 return true; 00659 } 00660 00661 /*****************************************************************************/ 00662 /* Hessian requires 253 flops. */ 00663 /* Reductions possible when b == 1, c == 1, or d == 1 */ 00664 /*****************************************************************************/ 00665 inline bool h_fcn_2i( double& obj, Vector3D g_obj[3], Matrix3D h_obj[6], const Vector3D x[3], const Vector3D& n, 00666 const double a, const Exponent& b, const Exponent& c, const Vector3D& d ) 00667 { 00668 double matr[9], f; 00669 double adj_m[9], g; // adj_m[2,5,8] not used 00670 double dg[9]; // dg[2,5,8] not used 00671 double loc0, loc1, loc2, loc3, loc4; 00672 double A[12], J_A[6], J_B[9], J_C[9], cross; // only 2x2 corners used 00673 00674 const double scale[3] = { d[0] * d[0], d[0] * d[1], d[1] * d[1] }; 00675 00676 /* Calculate M = [A*inv(W) n] */ 00677 matr[0] = d[0] * ( x[1][0] - x[0][0] ); 00678 matr[1] = d[1] * ( x[2][0] - x[0][0] ); 00679 matr[2] = n[0]; 00680 00681 matr[3] = d[0] * ( x[1][1] - x[0][1] ); 00682 matr[4] = d[1] * ( x[2][1] - x[0][1] ); 00683 matr[5] = n[1]; 00684 00685 matr[6] = d[0] * ( x[1][2] - x[0][2] ); 00686 matr[7] = d[1] * ( x[2][2] - x[0][2] ); 00687 matr[8] = n[2]; 00688 00689 /* Calculate det([n M]). */ 00690 dg[0] = matr[4] * matr[8] - matr[5] * matr[7]; 00691 dg[3] = matr[2] * matr[7] - matr[1] * matr[8]; 00692 dg[6] = matr[1] * matr[5] - matr[2] * matr[4]; 00693 g = matr[0] * dg[0] + matr[3] * dg[3] + matr[6] * dg[6]; 00694 if( g < MSQ_MIN ) 00695 { 00696 obj = g; 00697 return false; 00698 } 00699 00700 /* Calculate norm(M). */ 00701 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[3] * matr[3] + matr[4] * matr[4] + matr[6] * matr[6] + 00702 matr[7] * matr[7]; 00703 00704 loc3 = f; 00705 loc4 = g; 00706 00707 /* Calculate objective function. */ 00708 obj = a * b.raise( f ) * c.raise( g ); 00709 00710 /* Calculate the derivative of the objective function. */ 00711 f = b.value() * obj / f; /* Constant on nabla f */ 00712 g = c.value() * obj / g; /* Constant on nable g */ 00713 f *= 2.0; /* Modification for nabla f */ 00714 00715 dg[1] = matr[5] * matr[6] - matr[3] * matr[8]; 00716 dg[4] = matr[0] * matr[8] - matr[2] * matr[6]; 00717 dg[7] = matr[2] * matr[3] - matr[0] * matr[5]; 00718 00719 adj_m[0] = d[0] * ( matr[0] * f + dg[0] * g ); 00720 adj_m[1] = d[1] * ( matr[1] * f + dg[1] * g ); 00721 adj_m[3] = d[0] * ( matr[3] * f + dg[3] * g ); 00722 adj_m[4] = d[1] * ( matr[4] * f + dg[4] * g ); 00723 adj_m[6] = d[0] * ( matr[6] * f + dg[6] * g ); 00724 adj_m[7] = d[1] * ( matr[7] * f + dg[7] * g ); 00725 00726 g_obj[0][0] = -adj_m[0] - adj_m[1]; 00727 g_obj[1][0] = adj_m[0]; 00728 g_obj[2][0] = adj_m[1]; 00729 00730 g_obj[0][1] = -adj_m[3] - adj_m[4]; 00731 g_obj[1][1] = adj_m[3]; 00732 g_obj[2][1] = adj_m[4]; 00733 00734 g_obj[0][2] = -adj_m[6] - adj_m[7]; 00735 g_obj[1][2] = adj_m[6]; 00736 g_obj[2][2] = adj_m[7]; 00737 00738 /* Calculate the hessian of the objective. */ 00739 loc0 = f; /* Constant on nabla^2 f */ 00740 loc1 = g; /* Constant on nabla^2 g */ 00741 cross = f * c.value() / loc4; /* Constant on nabla g nabla f */ 00742 f = f * ( b.value() - 1 ) / loc3; /* Constant on nabla f nabla f */ 00743 g = g * ( c.value() - 1 ) / loc4; /* Constant on nabla g nabla g */ 00744 f *= 2.0; /* Modification for nabla f */ 00745 00746 /* First block of rows */ 00747 loc3 = matr[0] * f + dg[0] * cross; 00748 loc4 = dg[0] * g + matr[0] * cross; 00749 00750 J_A[0] = loc0 + loc3 * matr[0] + loc4 * dg[0]; 00751 J_A[1] = loc3 * matr[1] + loc4 * dg[1]; 00752 J_B[0] = loc3 * matr[3] + loc4 * dg[3]; 00753 J_B[1] = loc3 * matr[4] + loc4 * dg[4]; 00754 J_C[0] = loc3 * matr[6] + loc4 * dg[6]; 00755 J_C[1] = loc3 * matr[7] + loc4 * dg[7]; 00756 00757 loc3 = matr[1] * f + dg[1] * cross; 00758 loc4 = dg[1] * g + matr[1] * cross; 00759 00760 J_A[3] = loc0 + loc3 * matr[1] + loc4 * dg[1]; 00761 J_B[3] = loc3 * matr[3] + loc4 * dg[3]; 00762 J_B[4] = loc3 * matr[4] + loc4 * dg[4]; 00763 J_C[3] = loc3 * matr[6] + loc4 * dg[6]; 00764 J_C[4] = loc3 * matr[7] + loc4 * dg[7]; 00765 00766 /* First diagonal block */ 00767 J_A[0] *= scale[0]; 00768 J_A[1] *= scale[1]; 00769 J_A[3] *= scale[2]; 00770 00771 A[0] = -J_A[0] - J_A[1]; 00772 A[4] = -J_A[1] - J_A[3]; 00773 00774 h_obj[0][0][0] = -A[0] - A[4]; 00775 h_obj[1][0][0] = A[0]; 00776 h_obj[2][0][0] = A[4]; 00777 00778 h_obj[3][0][0] = J_A[0]; 00779 h_obj[4][0][0] = J_A[1]; 00780 00781 h_obj[5][0][0] = J_A[3]; 00782 00783 /* First off-diagonal block */ 00784 loc2 = matr[8] * loc1; 00785 J_B[1] += loc2; 00786 J_B[3] -= loc2; 00787 00788 J_B[0] *= scale[0]; 00789 J_B[1] *= scale[1]; 00790 J_B[3] *= scale[1]; 00791 J_B[4] *= scale[2]; 00792 00793 A[0] = -J_B[0] - J_B[3]; 00794 A[4] = -J_B[1] - J_B[4]; 00795 00796 h_obj[0][0][1] = -A[0] - A[4]; 00797 h_obj[1][0][1] = A[0]; 00798 h_obj[2][0][1] = A[4]; 00799 00800 h_obj[1][1][0] = -J_B[0] - J_B[1]; 00801 h_obj[3][0][1] = J_B[0]; 00802 h_obj[4][0][1] = J_B[1]; 00803 00804 h_obj[2][1][0] = -J_B[3] - J_B[4]; 00805 h_obj[4][1][0] = J_B[3]; 00806 h_obj[5][0][1] = J_B[4]; 00807 00808 /* Second off-diagonal block */ 00809 loc2 = matr[5] * loc1; 00810 J_C[1] -= loc2; 00811 J_C[3] += loc2; 00812 00813 J_C[0] *= scale[0]; 00814 J_C[1] *= scale[1]; 00815 J_C[3] *= scale[1]; 00816 J_C[4] *= scale[2]; 00817 00818 A[0] = -J_C[0] - J_C[3]; 00819 A[4] = -J_C[1] - J_C[4]; 00820 00821 h_obj[0][0][2] = -A[0] - A[4]; 00822 h_obj[1][0][2] = A[0]; 00823 h_obj[2][0][2] = A[4]; 00824 00825 h_obj[1][2][0] = -J_C[0] - J_C[1]; 00826 h_obj[3][0][2] = J_C[0]; 00827 h_obj[4][0][2] = J_C[1]; 00828 00829 h_obj[2][2][0] = -J_C[3] - J_C[4]; 00830 h_obj[4][2][0] = J_C[3]; 00831 h_obj[5][0][2] = J_C[4]; 00832 00833 /* Second block of rows */ 00834 loc3 = matr[3] * f + dg[3] * cross; 00835 loc4 = dg[3] * g + matr[3] * cross; 00836 00837 J_A[0] = loc0 + loc3 * matr[3] + loc4 * dg[3]; 00838 J_A[1] = loc3 * matr[4] + loc4 * dg[4]; 00839 J_B[0] = loc3 * matr[6] + loc4 * dg[6]; 00840 J_B[1] = loc3 * matr[7] + loc4 * dg[7]; 00841 00842 loc3 = matr[4] * f + dg[4] * cross; 00843 loc4 = dg[4] * g + matr[4] * cross; 00844 00845 J_A[3] = loc0 + loc3 * matr[4] + loc4 * dg[4]; 00846 J_B[3] = loc3 * matr[6] + loc4 * dg[6]; 00847 J_B[4] = loc3 * matr[7] + loc4 * dg[7]; 00848 00849 /* Second diagonal block */ 00850 J_A[0] *= scale[0]; 00851 J_A[1] *= scale[1]; 00852 J_A[3] *= scale[2]; 00853 00854 A[0] = -J_A[0] - J_A[1]; 00855 A[4] = -J_A[1] - J_A[3]; 00856 00857 h_obj[0][1][1] = -A[0] - A[4]; 00858 h_obj[1][1][1] = A[0]; 00859 h_obj[2][1][1] = A[4]; 00860 00861 h_obj[3][1][1] = J_A[0]; 00862 h_obj[4][1][1] = J_A[1]; 00863 00864 h_obj[5][1][1] = J_A[3]; 00865 00866 /* Third off-diagonal block */ 00867 loc2 = matr[2] * loc1; 00868 J_B[1] += loc2; 00869 J_B[3] -= loc2; 00870 00871 J_B[0] *= scale[0]; 00872 J_B[1] *= scale[1]; 00873 J_B[3] *= scale[1]; 00874 J_B[4] *= scale[2]; 00875 00876 A[0] = -J_B[0] - J_B[3]; 00877 A[4] = -J_B[1] - J_B[4]; 00878 00879 h_obj[0][1][2] = -A[0] - A[4]; 00880 h_obj[1][1][2] = A[0]; 00881 h_obj[2][1][2] = A[4]; 00882 00883 h_obj[1][2][1] = -J_B[0] - J_B[1]; 00884 h_obj[3][1][2] = J_B[0]; 00885 h_obj[4][1][2] = J_B[1]; 00886 00887 h_obj[2][2][1] = -J_B[3] - J_B[4]; 00888 h_obj[4][2][1] = J_B[3]; 00889 h_obj[5][1][2] = J_B[4]; 00890 00891 /* Third block of rows */ 00892 loc3 = matr[6] * f + dg[6] * cross; 00893 loc4 = dg[6] * g + matr[6] * cross; 00894 00895 J_A[0] = loc0 + loc3 * matr[6] + loc4 * dg[6]; 00896 J_A[1] = loc3 * matr[7] + loc4 * dg[7]; 00897 00898 loc3 = matr[7] * f + dg[7] * cross; 00899 loc4 = dg[7] * g + matr[7] * cross; 00900 00901 J_A[3] = loc0 + loc3 * matr[7] + loc4 * dg[7]; 00902 00903 /* Third diagonal block */ 00904 J_A[0] *= scale[0]; 00905 J_A[1] *= scale[1]; 00906 J_A[3] *= scale[2]; 00907 00908 A[0] = -J_A[0] - J_A[1]; 00909 A[4] = -J_A[1] - J_A[3]; 00910 00911 h_obj[0][2][2] = -A[0] - A[4]; 00912 h_obj[1][2][2] = A[0]; 00913 h_obj[2][2][2] = A[4]; 00914 00915 h_obj[3][2][2] = J_A[0]; 00916 h_obj[4][2][2] = J_A[1]; 00917 00918 h_obj[5][2][2] = J_A[3]; 00919 00920 // completes diagonal blocks. 00921 h_obj[0].fill_lower_triangle(); 00922 h_obj[3].fill_lower_triangle(); 00923 h_obj[5].fill_lower_triangle(); 00924 return true; 00925 } 00926 00927 /*****************************************************************************/ 00928 /* The following set of functions reference tetrahedral elements to a */ 00929 /* regular tetrahedron. They are used when assessing the quality of a */ 00930 /* tetrahedral element. A zero return value indicates success, while */ 00931 /* a nonzero value indicates failure. */ 00932 /*****************************************************************************/ 00933 00934 /*****************************************************************************/ 00935 /* Function evaluation requires 62 flops. */ 00936 /* Reductions possible when b == 1 or c == 1 */ 00937 /*****************************************************************************/ 00938 inline bool m_fcn_3e( double& obj, const Vector3D x[4], const double a, const Exponent& b, const Exponent& c ) 00939 { 00940 double matr[9], f; 00941 double g; 00942 00943 /* Calculate M = A*inv(W). */ 00944 f = x[1][0] + x[0][0]; 00945 matr[0] = x[1][0] - x[0][0]; 00946 matr[1] = ( 2.0 * x[2][0] - f ) * isqrt3; 00947 matr[2] = ( 3.0 * x[3][0] - x[2][0] - f ) * isqrt6; 00948 00949 f = x[1][1] + x[0][1]; 00950 matr[3] = x[1][1] - x[0][1]; 00951 matr[4] = ( 2.0 * x[2][1] - f ) * isqrt3; 00952 matr[5] = ( 3.0 * x[3][1] - x[2][1] - f ) * isqrt6; 00953 00954 f = x[1][2] + x[0][2]; 00955 matr[6] = x[1][2] - x[0][2]; 00956 matr[7] = ( 2.0 * x[2][2] - f ) * isqrt3; 00957 matr[8] = ( 3.0 * x[3][2] - x[2][2] - f ) * isqrt6; 00958 00959 /* Calculate det(M). */ 00960 g = matr[0] * ( matr[4] * matr[8] - matr[5] * matr[7] ) + matr[1] * ( matr[5] * matr[6] - matr[3] * matr[8] ) + 00961 matr[2] * ( matr[3] * matr[7] - matr[4] * matr[6] ); 00962 if( g < MSQ_MIN ) 00963 { 00964 obj = g; 00965 return false; 00966 } 00967 00968 /* Calculate norm(M). */ 00969 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 00970 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 00971 00972 /* Calculate objective function. */ 00973 obj = a * b.raise( f ) * c.raise( g ); 00974 return true; 00975 } 00976 00977 /*****************************************************************************/ 00978 /* Gradient evaluation requires 133 flops. */ 00979 /* Reductions possible when b == 1 or c == 1 */ 00980 /*****************************************************************************/ 00981 inline bool g_fcn_3e( double& obj, Vector3D g_obj[4], const Vector3D x[4], const double a, const Exponent& b, 00982 const Exponent& c ) 00983 { 00984 double matr[9], f; 00985 double adj_m[9], g; 00986 double loc1, loc2, loc3; 00987 00988 /* Calculate M = A*inv(W). */ 00989 f = x[1][0] + x[0][0]; 00990 matr[0] = x[1][0] - x[0][0]; 00991 matr[1] = ( 2.0 * x[2][0] - f ) * isqrt3; 00992 matr[2] = ( 3.0 * x[3][0] - x[2][0] - f ) * isqrt6; 00993 00994 f = x[1][1] + x[0][1]; 00995 matr[3] = x[1][1] - x[0][1]; 00996 matr[4] = ( 2.0 * x[2][1] - f ) * isqrt3; 00997 matr[5] = ( 3.0 * x[3][1] - x[2][1] - f ) * isqrt6; 00998 00999 f = x[1][2] + x[0][2]; 01000 matr[6] = x[1][2] - x[0][2]; 01001 matr[7] = ( 2.0 * x[2][2] - f ) * isqrt3; 01002 matr[8] = ( 3.0 * x[3][2] - x[2][2] - f ) * isqrt6; 01003 01004 /* Calculate det(M). */ 01005 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 01006 loc2 = matr[5] * matr[6] - matr[3] * matr[8]; 01007 loc3 = matr[3] * matr[7] - matr[4] * matr[6]; 01008 g = matr[0] * loc1 + matr[1] * loc2 + matr[2] * loc3; 01009 if( g < MSQ_MIN ) 01010 { 01011 obj = g; 01012 return false; 01013 } 01014 01015 /* Calculate norm(M). */ 01016 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 01017 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 01018 01019 /* Calculate objective function. */ 01020 obj = a * b.raise( f ) * c.raise( g ); 01021 01022 /* Calculate the derivative of the objective function. */ 01023 f = b.value() * obj / f; /* Constant on nabla f */ 01024 g = c.value() * obj / g; /* Constant on nable g */ 01025 f *= 2.0; /* Modification for nabla f */ 01026 01027 adj_m[0] = matr[0] * f + loc1 * g; 01028 adj_m[1] = matr[1] * f + loc2 * g; 01029 adj_m[2] = matr[2] * f + loc3 * g; 01030 01031 loc1 = matr[0] * g; 01032 loc2 = matr[1] * g; 01033 loc3 = matr[2] * g; 01034 01035 adj_m[3] = matr[3] * f + loc3 * matr[7] - loc2 * matr[8]; 01036 adj_m[4] = matr[4] * f + loc1 * matr[8] - loc3 * matr[6]; 01037 adj_m[5] = matr[5] * f + loc2 * matr[6] - loc1 * matr[7]; 01038 01039 adj_m[6] = matr[6] * f + loc2 * matr[5] - loc3 * matr[4]; 01040 adj_m[7] = matr[7] * f + loc3 * matr[3] - loc1 * matr[5]; 01041 adj_m[8] = matr[8] * f + loc1 * matr[4] - loc2 * matr[3]; 01042 01043 loc1 = isqrt3 * adj_m[1]; 01044 loc2 = isqrt6 * adj_m[2]; 01045 loc3 = loc1 + loc2; 01046 g_obj[0][0] = -adj_m[0] - loc3; 01047 g_obj[1][0] = adj_m[0] - loc3; 01048 g_obj[2][0] = 2.0 * loc1 - loc2; 01049 g_obj[3][0] = 3.0 * loc2; 01050 01051 loc1 = isqrt3 * adj_m[4]; 01052 loc2 = isqrt6 * adj_m[5]; 01053 loc3 = loc1 + loc2; 01054 g_obj[0][1] = -adj_m[3] - loc3; 01055 g_obj[1][1] = adj_m[3] - loc3; 01056 g_obj[2][1] = 2.0 * loc1 - loc2; 01057 g_obj[3][1] = 3.0 * loc2; 01058 01059 loc1 = isqrt3 * adj_m[7]; 01060 loc2 = isqrt6 * adj_m[8]; 01061 loc3 = loc1 + loc2; 01062 g_obj[0][2] = -adj_m[6] - loc3; 01063 g_obj[1][2] = adj_m[6] - loc3; 01064 g_obj[2][2] = 2.0 * loc1 - loc2; 01065 g_obj[3][2] = 3.0 * loc2; 01066 return true; 01067 } 01068 01069 /*****************************************************************************/ 01070 /* Hessian evaluation requires 634 flops. */ 01071 /* Reductions possible when b == 1 or c == 1 */ 01072 /*****************************************************************************/ 01073 inline bool h_fcn_3e( double& obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const double a, 01074 const Exponent& b, const Exponent& c ) 01075 { 01076 double matr[9], f; 01077 double adj_m[9], g; 01078 double dg[9], loc0, loc1, loc2, loc3, loc4; 01079 double A[12], J_A[6], J_B[9], J_C[9], cross; 01080 01081 /* Calculate M = A*inv(W). */ 01082 f = x[1][0] + x[0][0]; 01083 matr[0] = x[1][0] - x[0][0]; 01084 matr[1] = ( 2.0 * x[2][0] - f ) * isqrt3; 01085 matr[2] = ( 3.0 * x[3][0] - x[2][0] - f ) * isqrt6; 01086 01087 f = x[1][1] + x[0][1]; 01088 matr[3] = x[1][1] - x[0][1]; 01089 matr[4] = ( 2.0 * x[2][1] - f ) * isqrt3; 01090 matr[5] = ( 3.0 * x[3][1] - x[2][1] - f ) * isqrt6; 01091 01092 f = x[1][2] + x[0][2]; 01093 matr[6] = x[1][2] - x[0][2]; 01094 matr[7] = ( 2.0 * x[2][2] - f ) * isqrt3; 01095 matr[8] = ( 3.0 * x[3][2] - x[2][2] - f ) * isqrt6; 01096 01097 /* Calculate det(M). */ 01098 dg[0] = matr[4] * matr[8] - matr[5] * matr[7]; 01099 dg[1] = matr[5] * matr[6] - matr[3] * matr[8]; 01100 dg[2] = matr[3] * matr[7] - matr[4] * matr[6]; 01101 g = matr[0] * dg[0] + matr[1] * dg[1] + matr[2] * dg[2]; 01102 if( g < MSQ_MIN ) 01103 { 01104 obj = g; 01105 return false; 01106 } 01107 01108 /* Calculate norm(M). */ 01109 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 01110 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 01111 01112 loc3 = f; 01113 loc4 = g; 01114 01115 /* Calculate objective function. */ 01116 obj = a * b.raise( f ) * c.raise( g ); 01117 01118 /* Calculate the derivative of the objective function. */ 01119 f = b.value() * obj / f; /* Constant on nabla f */ 01120 g = c.value() * obj / g; /* Constant on nable g */ 01121 f *= 2.0; /* Modification for nabla f */ 01122 01123 dg[3] = matr[2] * matr[7] - matr[1] * matr[8]; 01124 dg[4] = matr[0] * matr[8] - matr[2] * matr[6]; 01125 dg[5] = matr[1] * matr[6] - matr[0] * matr[7]; 01126 dg[6] = matr[1] * matr[5] - matr[2] * matr[4]; 01127 dg[7] = matr[2] * matr[3] - matr[0] * matr[5]; 01128 dg[8] = matr[0] * matr[4] - matr[1] * matr[3]; 01129 01130 adj_m[0] = matr[0] * f + dg[0] * g; 01131 adj_m[1] = matr[1] * f + dg[1] * g; 01132 adj_m[2] = matr[2] * f + dg[2] * g; 01133 adj_m[3] = matr[3] * f + dg[3] * g; 01134 adj_m[4] = matr[4] * f + dg[4] * g; 01135 adj_m[5] = matr[5] * f + dg[5] * g; 01136 adj_m[6] = matr[6] * f + dg[6] * g; 01137 adj_m[7] = matr[7] * f + dg[7] * g; 01138 adj_m[8] = matr[8] * f + dg[8] * g; 01139 01140 loc0 = isqrt3 * adj_m[1]; 01141 loc1 = isqrt6 * adj_m[2]; 01142 loc2 = loc0 + loc1; 01143 g_obj[0][0] = -adj_m[0] - loc2; 01144 g_obj[1][0] = adj_m[0] - loc2; 01145 g_obj[2][0] = 2.0 * loc0 - loc1; 01146 g_obj[3][0] = 3.0 * loc1; 01147 01148 loc0 = isqrt3 * adj_m[4]; 01149 loc1 = isqrt6 * adj_m[5]; 01150 loc2 = loc0 + loc1; 01151 g_obj[0][1] = -adj_m[3] - loc2; 01152 g_obj[1][1] = adj_m[3] - loc2; 01153 g_obj[2][1] = 2.0 * loc0 - loc1; 01154 g_obj[3][1] = 3.0 * loc1; 01155 01156 loc0 = isqrt3 * adj_m[7]; 01157 loc1 = isqrt6 * adj_m[8]; 01158 loc2 = loc0 + loc1; 01159 g_obj[0][2] = -adj_m[6] - loc2; 01160 g_obj[1][2] = adj_m[6] - loc2; 01161 g_obj[2][2] = 2.0 * loc0 - loc1; 01162 g_obj[3][2] = 3.0 * loc1; 01163 01164 /* Calculate the hessian of the objective. */ 01165 loc0 = f; /* Constant on nabla^2 f */ 01166 loc1 = g; /* Constant on nabla^2 g */ 01167 cross = f * c.value() / loc4; /* Constant on nabla g nabla f */ 01168 f = f * ( b.value() - 1 ) / loc3; /* Constant on nabla f nabla f */ 01169 g = g * ( c.value() - 1 ) / loc4; /* Constant on nabla g nabla g */ 01170 f *= 2.0; /* Modification for nabla f */ 01171 01172 /* First block of rows */ 01173 loc3 = matr[0] * f + dg[0] * cross; 01174 loc4 = dg[0] * g + matr[0] * cross; 01175 01176 J_A[0] = loc0 + loc3 * matr[0] + loc4 * dg[0]; 01177 J_A[1] = loc3 * matr[1] + loc4 * dg[1]; 01178 J_A[2] = loc3 * matr[2] + loc4 * dg[2]; 01179 J_B[0] = loc3 * matr[3] + loc4 * dg[3]; 01180 J_B[1] = loc3 * matr[4] + loc4 * dg[4]; 01181 J_B[2] = loc3 * matr[5] + loc4 * dg[5]; 01182 J_C[0] = loc3 * matr[6] + loc4 * dg[6]; 01183 J_C[1] = loc3 * matr[7] + loc4 * dg[7]; 01184 J_C[2] = loc3 * matr[8] + loc4 * dg[8]; 01185 01186 loc3 = matr[1] * f + dg[1] * cross; 01187 loc4 = dg[1] * g + matr[1] * cross; 01188 01189 J_A[3] = loc0 + loc3 * matr[1] + loc4 * dg[1]; 01190 J_A[4] = loc3 * matr[2] + loc4 * dg[2]; 01191 J_B[3] = loc3 * matr[3] + loc4 * dg[3]; 01192 J_B[4] = loc3 * matr[4] + loc4 * dg[4]; 01193 J_B[5] = loc3 * matr[5] + loc4 * dg[5]; 01194 J_C[3] = loc3 * matr[6] + loc4 * dg[6]; 01195 J_C[4] = loc3 * matr[7] + loc4 * dg[7]; 01196 J_C[5] = loc3 * matr[8] + loc4 * dg[8]; 01197 01198 loc3 = matr[2] * f + dg[2] * cross; 01199 loc4 = dg[2] * g + matr[2] * cross; 01200 01201 J_A[5] = loc0 + loc3 * matr[2] + loc4 * dg[2]; 01202 J_B[6] = loc3 * matr[3] + loc4 * dg[3]; 01203 J_B[7] = loc3 * matr[4] + loc4 * dg[4]; 01204 J_B[8] = loc3 * matr[5] + loc4 * dg[5]; 01205 J_C[6] = loc3 * matr[6] + loc4 * dg[6]; 01206 J_C[7] = loc3 * matr[7] + loc4 * dg[7]; 01207 J_C[8] = loc3 * matr[8] + loc4 * dg[8]; 01208 01209 /* First diagonal block */ 01210 loc2 = isqrt3 * J_A[1]; 01211 loc3 = isqrt6 * J_A[2]; 01212 loc4 = loc2 + loc3; 01213 01214 A[0] = -J_A[0] - loc4; 01215 A[1] = J_A[0] - loc4; 01216 01217 loc2 = isqrt3 * J_A[3]; 01218 loc3 = isqrt6 * J_A[4]; 01219 loc4 = loc2 + loc3; 01220 01221 A[4] = -J_A[1] - loc4; 01222 A[5] = J_A[1] - loc4; 01223 A[6] = 2.0 * loc2 - loc3; 01224 01225 loc2 = isqrt3 * J_A[4]; 01226 loc3 = isqrt6 * J_A[5]; 01227 loc4 = loc2 + loc3; 01228 01229 A[8] = -J_A[2] - loc4; 01230 A[9] = J_A[2] - loc4; 01231 A[10] = 2.0 * loc2 - loc3; 01232 A[11] = 3.0 * loc3; 01233 01234 loc2 = isqrt3 * A[4]; 01235 loc3 = isqrt6 * A[8]; 01236 loc4 = loc2 + loc3; 01237 01238 h_obj[0][0][0] = -A[0] - loc4; 01239 h_obj[1][0][0] = A[0] - loc4; 01240 h_obj[2][0][0] = 2.0 * loc2 - loc3; 01241 h_obj[3][0][0] = 3.0 * loc3; 01242 01243 loc2 = isqrt3 * A[5]; 01244 loc3 = isqrt6 * A[9]; 01245 01246 h_obj[4][0][0] = A[1] - loc2 - loc3; 01247 h_obj[5][0][0] = 2.0 * loc2 - loc3; 01248 h_obj[6][0][0] = 3.0 * loc3; 01249 01250 loc3 = isqrt6 * A[10]; 01251 h_obj[7][0][0] = tisqrt3 * A[6] - loc3; 01252 h_obj[8][0][0] = 3.0 * loc3; 01253 01254 h_obj[9][0][0] = tisqrt6 * A[11]; 01255 01256 /* First off-diagonal block */ 01257 loc2 = matr[8] * loc1; 01258 J_B[1] += loc2; 01259 J_B[3] -= loc2; 01260 01261 loc2 = matr[7] * loc1; 01262 J_B[2] -= loc2; 01263 J_B[6] += loc2; 01264 01265 loc2 = matr[6] * loc1; 01266 J_B[5] += loc2; 01267 J_B[7] -= loc2; 01268 01269 loc2 = isqrt3 * J_B[3]; 01270 loc3 = isqrt6 * J_B[6]; 01271 loc4 = loc2 + loc3; 01272 01273 A[0] = -J_B[0] - loc4; 01274 A[1] = J_B[0] - loc4; 01275 A[2] = 2.0 * loc2 - loc3; 01276 A[3] = 3.0 * loc3; 01277 01278 loc2 = isqrt3 * J_B[4]; 01279 loc3 = isqrt6 * J_B[7]; 01280 loc4 = loc2 + loc3; 01281 01282 A[4] = -J_B[1] - loc4; 01283 A[5] = J_B[1] - loc4; 01284 A[6] = 2.0 * loc2 - loc3; 01285 A[7] = 3.0 * loc3; 01286 01287 loc2 = isqrt3 * J_B[5]; 01288 loc3 = isqrt6 * J_B[8]; 01289 loc4 = loc2 + loc3; 01290 01291 A[8] = -J_B[2] - loc4; 01292 A[9] = J_B[2] - loc4; 01293 A[10] = 2.0 * loc2 - loc3; 01294 A[11] = 3.0 * loc3; 01295 01296 loc2 = isqrt3 * A[4]; 01297 loc3 = isqrt6 * A[8]; 01298 loc4 = loc2 + loc3; 01299 01300 h_obj[0][0][1] = -A[0] - loc4; 01301 h_obj[1][0][1] = A[0] - loc4; 01302 h_obj[2][0][1] = 2.0 * loc2 - loc3; 01303 h_obj[3][0][1] = 3.0 * loc3; 01304 01305 loc2 = isqrt3 * A[5]; 01306 loc3 = isqrt6 * A[9]; 01307 loc4 = loc2 + loc3; 01308 01309 h_obj[1][1][0] = -A[1] - loc4; 01310 h_obj[4][0][1] = A[1] - loc4; 01311 h_obj[5][0][1] = 2.0 * loc2 - loc3; 01312 h_obj[6][0][1] = 3.0 * loc3; 01313 01314 loc2 = isqrt3 * A[6]; 01315 loc3 = isqrt6 * A[10]; 01316 loc4 = loc2 + loc3; 01317 01318 h_obj[2][1][0] = -A[2] - loc4; 01319 h_obj[5][1][0] = A[2] - loc4; 01320 h_obj[7][0][1] = 2.0 * loc2 - loc3; 01321 h_obj[8][0][1] = 3.0 * loc3; 01322 01323 loc2 = isqrt3 * A[7]; 01324 loc3 = isqrt6 * A[11]; 01325 loc4 = loc2 + loc3; 01326 01327 h_obj[3][1][0] = -A[3] - loc4; 01328 h_obj[6][1][0] = A[3] - loc4; 01329 h_obj[8][1][0] = 2.0 * loc2 - loc3; 01330 h_obj[9][0][1] = 3.0 * loc3; 01331 01332 /* Second off-diagonal block */ 01333 loc2 = matr[5] * loc1; 01334 J_C[1] -= loc2; 01335 J_C[3] += loc2; 01336 01337 loc2 = matr[4] * loc1; 01338 J_C[2] += loc2; 01339 J_C[6] -= loc2; 01340 01341 loc2 = matr[3] * loc1; 01342 J_C[5] -= loc2; 01343 J_C[7] += loc2; 01344 01345 loc2 = isqrt3 * J_C[3]; 01346 loc3 = isqrt6 * J_C[6]; 01347 loc4 = loc2 + loc3; 01348 01349 A[0] = -J_C[0] - loc4; 01350 A[1] = J_C[0] - loc4; 01351 A[2] = 2.0 * loc2 - loc3; 01352 A[3] = 3.0 * loc3; 01353 01354 loc2 = isqrt3 * J_C[4]; 01355 loc3 = isqrt6 * J_C[7]; 01356 loc4 = loc2 + loc3; 01357 01358 A[4] = -J_C[1] - loc4; 01359 A[5] = J_C[1] - loc4; 01360 A[6] = 2.0 * loc2 - loc3; 01361 A[7] = 3.0 * loc3; 01362 01363 loc2 = isqrt3 * J_C[5]; 01364 loc3 = isqrt6 * J_C[8]; 01365 loc4 = loc2 + loc3; 01366 01367 A[8] = -J_C[2] - loc4; 01368 A[9] = J_C[2] - loc4; 01369 A[10] = 2.0 * loc2 - loc3; 01370 A[11] = 3.0 * loc3; 01371 01372 loc2 = isqrt3 * A[4]; 01373 loc3 = isqrt6 * A[8]; 01374 loc4 = loc2 + loc3; 01375 01376 h_obj[0][0][2] = -A[0] - loc4; 01377 h_obj[1][0][2] = A[0] - loc4; 01378 h_obj[2][0][2] = 2.0 * loc2 - loc3; 01379 h_obj[3][0][2] = 3.0 * loc3; 01380 01381 loc2 = isqrt3 * A[5]; 01382 loc3 = isqrt6 * A[9]; 01383 loc4 = loc2 + loc3; 01384 01385 h_obj[1][2][0] = -A[1] - loc4; 01386 h_obj[4][0][2] = A[1] - loc4; 01387 h_obj[5][0][2] = 2.0 * loc2 - loc3; 01388 h_obj[6][0][2] = 3.0 * loc3; 01389 01390 loc2 = isqrt3 * A[6]; 01391 loc3 = isqrt6 * A[10]; 01392 loc4 = loc2 + loc3; 01393 01394 h_obj[2][2][0] = -A[2] - loc4; 01395 h_obj[5][2][0] = A[2] - loc4; 01396 h_obj[7][0][2] = 2.0 * loc2 - loc3; 01397 h_obj[8][0][2] = 3.0 * loc3; 01398 01399 loc2 = isqrt3 * A[7]; 01400 loc3 = isqrt6 * A[11]; 01401 loc4 = loc2 + loc3; 01402 01403 h_obj[3][2][0] = -A[3] - loc4; 01404 h_obj[6][2][0] = A[3] - loc4; 01405 h_obj[8][2][0] = 2.0 * loc2 - loc3; 01406 h_obj[9][0][2] = 3.0 * loc3; 01407 01408 /* Second block of rows */ 01409 loc3 = matr[3] * f + dg[3] * cross; 01410 loc4 = dg[3] * g + matr[3] * cross; 01411 01412 J_A[0] = loc0 + loc3 * matr[3] + loc4 * dg[3]; 01413 J_A[1] = loc3 * matr[4] + loc4 * dg[4]; 01414 J_A[2] = loc3 * matr[5] + loc4 * dg[5]; 01415 J_B[0] = loc3 * matr[6] + loc4 * dg[6]; 01416 J_B[1] = loc3 * matr[7] + loc4 * dg[7]; 01417 J_B[2] = loc3 * matr[8] + loc4 * dg[8]; 01418 01419 loc3 = matr[4] * f + dg[4] * cross; 01420 loc4 = dg[4] * g + matr[4] * cross; 01421 01422 J_A[3] = loc0 + loc3 * matr[4] + loc4 * dg[4]; 01423 J_A[4] = loc3 * matr[5] + loc4 * dg[5]; 01424 J_B[3] = loc3 * matr[6] + loc4 * dg[6]; 01425 J_B[4] = loc3 * matr[7] + loc4 * dg[7]; 01426 J_B[5] = loc3 * matr[8] + loc4 * dg[8]; 01427 01428 loc3 = matr[5] * f + dg[5] * cross; 01429 loc4 = dg[5] * g + matr[5] * cross; 01430 01431 J_A[5] = loc0 + loc3 * matr[5] + loc4 * dg[5]; 01432 J_B[6] = loc3 * matr[6] + loc4 * dg[6]; 01433 J_B[7] = loc3 * matr[7] + loc4 * dg[7]; 01434 J_B[8] = loc3 * matr[8] + loc4 * dg[8]; 01435 01436 /* Second diagonal block */ 01437 loc2 = isqrt3 * J_A[1]; 01438 loc3 = isqrt6 * J_A[2]; 01439 loc4 = loc2 + loc3; 01440 01441 A[0] = -J_A[0] - loc4; 01442 A[1] = J_A[0] - loc4; 01443 01444 loc2 = isqrt3 * J_A[3]; 01445 loc3 = isqrt6 * J_A[4]; 01446 loc4 = loc2 + loc3; 01447 01448 A[4] = -J_A[1] - loc4; 01449 A[5] = J_A[1] - loc4; 01450 A[6] = 2.0 * loc2 - loc3; 01451 01452 loc2 = isqrt3 * J_A[4]; 01453 loc3 = isqrt6 * J_A[5]; 01454 loc4 = loc2 + loc3; 01455 01456 A[8] = -J_A[2] - loc4; 01457 A[9] = J_A[2] - loc4; 01458 A[10] = 2.0 * loc2 - loc3; 01459 A[11] = 3.0 * loc3; 01460 01461 loc2 = isqrt3 * A[4]; 01462 loc3 = isqrt6 * A[8]; 01463 loc4 = loc2 + loc3; 01464 01465 h_obj[0][1][1] = -A[0] - loc4; 01466 h_obj[1][1][1] = A[0] - loc4; 01467 h_obj[2][1][1] = 2.0 * loc2 - loc3; 01468 h_obj[3][1][1] = 3.0 * loc3; 01469 01470 loc2 = isqrt3 * A[5]; 01471 loc3 = isqrt6 * A[9]; 01472 01473 h_obj[4][1][1] = A[1] - loc2 - loc3; 01474 h_obj[5][1][1] = 2.0 * loc2 - loc3; 01475 h_obj[6][1][1] = 3.0 * loc3; 01476 01477 loc3 = isqrt6 * A[10]; 01478 h_obj[7][1][1] = tisqrt3 * A[6] - loc3; 01479 h_obj[8][1][1] = 3.0 * loc3; 01480 01481 h_obj[9][1][1] = tisqrt6 * A[11]; 01482 01483 /* Third off-diagonal block */ 01484 loc2 = matr[2] * loc1; 01485 J_B[1] += loc2; 01486 J_B[3] -= loc2; 01487 01488 loc2 = matr[1] * loc1; 01489 J_B[2] -= loc2; 01490 J_B[6] += loc2; 01491 01492 loc2 = matr[0] * loc1; 01493 J_B[5] += loc2; 01494 J_B[7] -= loc2; 01495 01496 loc2 = isqrt3 * J_B[3]; 01497 loc3 = isqrt6 * J_B[6]; 01498 loc4 = loc2 + loc3; 01499 01500 A[0] = -J_B[0] - loc4; 01501 A[1] = J_B[0] - loc4; 01502 A[2] = 2.0 * loc2 - loc3; 01503 A[3] = 3.0 * loc3; 01504 01505 loc2 = isqrt3 * J_B[4]; 01506 loc3 = isqrt6 * J_B[7]; 01507 loc4 = loc2 + loc3; 01508 01509 A[4] = -J_B[1] - loc4; 01510 A[5] = J_B[1] - loc4; 01511 A[6] = 2.0 * loc2 - loc3; 01512 A[7] = 3.0 * loc3; 01513 01514 loc2 = isqrt3 * J_B[5]; 01515 loc3 = isqrt6 * J_B[8]; 01516 loc4 = loc2 + loc3; 01517 01518 A[8] = -J_B[2] - loc4; 01519 A[9] = J_B[2] - loc4; 01520 A[10] = 2.0 * loc2 - loc3; 01521 A[11] = 3.0 * loc3; 01522 01523 loc2 = isqrt3 * A[4]; 01524 loc3 = isqrt6 * A[8]; 01525 loc4 = loc2 + loc3; 01526 01527 h_obj[0][1][2] = -A[0] - loc4; 01528 h_obj[1][1][2] = A[0] - loc4; 01529 h_obj[2][1][2] = 2.0 * loc2 - loc3; 01530 h_obj[3][1][2] = 3.0 * loc3; 01531 01532 loc2 = isqrt3 * A[5]; 01533 loc3 = isqrt6 * A[9]; 01534 loc4 = loc2 + loc3; 01535 01536 h_obj[1][2][1] = -A[1] - loc4; 01537 h_obj[4][1][2] = A[1] - loc4; 01538 h_obj[5][1][2] = 2.0 * loc2 - loc3; 01539 h_obj[6][1][2] = 3.0 * loc3; 01540 01541 loc2 = isqrt3 * A[6]; 01542 loc3 = isqrt6 * A[10]; 01543 loc4 = loc2 + loc3; 01544 01545 h_obj[2][2][1] = -A[2] - loc4; 01546 h_obj[5][2][1] = A[2] - loc4; 01547 h_obj[7][1][2] = 2.0 * loc2 - loc3; 01548 h_obj[8][1][2] = 3.0 * loc3; 01549 01550 loc2 = isqrt3 * A[7]; 01551 loc3 = isqrt6 * A[11]; 01552 loc4 = loc2 + loc3; 01553 01554 h_obj[3][2][1] = -A[3] - loc4; 01555 h_obj[6][2][1] = A[3] - loc4; 01556 h_obj[8][2][1] = 2.0 * loc2 - loc3; 01557 h_obj[9][1][2] = 3.0 * loc3; 01558 01559 /* Third block of rows */ 01560 loc3 = matr[6] * f + dg[6] * cross; 01561 loc4 = dg[6] * g + matr[6] * cross; 01562 01563 J_A[0] = loc0 + loc3 * matr[6] + loc4 * dg[6]; 01564 J_A[1] = loc3 * matr[7] + loc4 * dg[7]; 01565 J_A[2] = loc3 * matr[8] + loc4 * dg[8]; 01566 01567 loc3 = matr[7] * f + dg[7] * cross; 01568 loc4 = dg[7] * g + matr[7] * cross; 01569 01570 J_A[3] = loc0 + loc3 * matr[7] + loc4 * dg[7]; 01571 J_A[4] = loc3 * matr[8] + loc4 * dg[8]; 01572 01573 loc3 = matr[8] * f + dg[8] * cross; 01574 loc4 = dg[8] * g + matr[8] * cross; 01575 01576 J_A[5] = loc0 + loc3 * matr[8] + loc4 * dg[8]; 01577 01578 /* Third diagonal block */ 01579 loc2 = isqrt3 * J_A[1]; 01580 loc3 = isqrt6 * J_A[2]; 01581 loc4 = loc2 + loc3; 01582 01583 A[0] = -J_A[0] - loc4; 01584 A[1] = J_A[0] - loc4; 01585 01586 loc2 = isqrt3 * J_A[3]; 01587 loc3 = isqrt6 * J_A[4]; 01588 loc4 = loc2 + loc3; 01589 01590 A[4] = -J_A[1] - loc4; 01591 A[5] = J_A[1] - loc4; 01592 A[6] = 2.0 * loc2 - loc3; 01593 01594 loc2 = isqrt3 * J_A[4]; 01595 loc3 = isqrt6 * J_A[5]; 01596 loc4 = loc2 + loc3; 01597 01598 A[8] = -J_A[2] - loc4; 01599 A[9] = J_A[2] - loc4; 01600 A[10] = 2.0 * loc2 - loc3; 01601 A[11] = 3.0 * loc3; 01602 01603 loc2 = isqrt3 * A[4]; 01604 loc3 = isqrt6 * A[8]; 01605 loc4 = loc2 + loc3; 01606 01607 h_obj[0][2][2] = -A[0] - loc4; 01608 h_obj[1][2][2] = A[0] - loc4; 01609 h_obj[2][2][2] = 2.0 * loc2 - loc3; 01610 h_obj[3][2][2] = 3.0 * loc3; 01611 01612 loc2 = isqrt3 * A[5]; 01613 loc3 = isqrt6 * A[9]; 01614 01615 h_obj[4][2][2] = A[1] - loc2 - loc3; 01616 h_obj[5][2][2] = 2.0 * loc2 - loc3; 01617 h_obj[6][2][2] = 3.0 * loc3; 01618 01619 loc3 = isqrt6 * A[10]; 01620 h_obj[7][2][2] = tisqrt3 * A[6] - loc3; 01621 h_obj[8][2][2] = 3.0 * loc3; 01622 01623 h_obj[9][2][2] = tisqrt6 * A[11]; 01624 01625 // completes diagonal blocks. 01626 h_obj[0].fill_lower_triangle(); 01627 h_obj[4].fill_lower_triangle(); 01628 h_obj[7].fill_lower_triangle(); 01629 h_obj[9].fill_lower_triangle(); 01630 return true; 01631 } 01632 01633 /*****************************************************************************/ 01634 /* The following set of functions reference tetrahedral elements to a */ 01635 /* equilateral tetrahedron. These functions are specialized toward */ 01636 /* obtaining the gradient and Hessian with respect to a single vertex. */ 01637 /*****************************************************************************/ 01638 01639 inline bool g_fcn_3e_v3( double& obj, Vector3D& g_obj, const Vector3D x[4], const double a, const Exponent& b, 01640 const Exponent& c ) 01641 { 01642 double matr[9], f, g; 01643 double loc1, loc2, loc3; 01644 01645 /* Calculate M = A*inv(W). */ 01646 f = x[1][0] + x[0][0]; 01647 matr[0] = x[1][0] - x[0][0]; 01648 matr[1] = ( 2.0 * x[2][0] - f ) * isqrt3; 01649 matr[2] = ( 3.0 * x[3][0] - x[2][0] - f ) * isqrt6; 01650 01651 f = x[1][1] + x[0][1]; 01652 matr[3] = x[1][1] - x[0][1]; 01653 matr[4] = ( 2.0 * x[2][1] - f ) * isqrt3; 01654 matr[5] = ( 3.0 * x[3][1] - x[2][1] - f ) * isqrt6; 01655 01656 f = x[1][2] + x[0][2]; 01657 matr[6] = x[1][2] - x[0][2]; 01658 matr[7] = ( 2.0 * x[2][2] - f ) * isqrt3; 01659 matr[8] = ( 3.0 * x[3][2] - x[2][2] - f ) * isqrt6; 01660 01661 /* Calculate det(M). */ 01662 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 01663 loc2 = matr[5] * matr[6] - matr[3] * matr[8]; 01664 loc3 = matr[3] * matr[7] - matr[4] * matr[6]; 01665 g = matr[0] * loc1 + matr[1] * loc2 + matr[2] * loc3; 01666 if( g < MSQ_MIN ) 01667 { 01668 obj = g; 01669 return false; 01670 } 01671 01672 /* Calculate norm(M). */ 01673 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 01674 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 01675 01676 /* Calculate objective function. */ 01677 obj = a * b.raise( f ) * c.raise( g ); 01678 01679 /* Calculate the derivative of the objective function. */ 01680 f = b.value() * obj / f; /* Constant on nabla f */ 01681 g = c.value() * obj / g; /* Constant on nable g */ 01682 f *= 2.0; /* Modification for nabla f */ 01683 01684 g_obj[0] = tisqrt6 * ( matr[2] * f + loc3 * g ); 01685 01686 loc1 = matr[0] * g; 01687 loc2 = matr[1] * g; 01688 01689 g_obj[1] = tisqrt6 * ( matr[5] * f + loc2 * matr[6] - loc1 * matr[7] ); 01690 g_obj[2] = tisqrt6 * ( matr[8] * f + loc1 * matr[4] - loc2 * matr[3] ); 01691 return true; 01692 } 01693 01694 inline bool g_fcn_3e_v0( double& obj, Vector3D& g_obj, const Vector3D x[4], const double a, const Exponent& b, 01695 const Exponent& c ) 01696 { 01697 static Vector3D my_x[4]; 01698 01699 my_x[0] = x[1]; 01700 my_x[1] = x[3]; 01701 my_x[2] = x[2]; 01702 my_x[3] = x[0]; 01703 return g_fcn_3e_v3( obj, g_obj, my_x, a, b, c ); 01704 } 01705 01706 inline bool g_fcn_3e_v1( double& obj, Vector3D& g_obj, const Vector3D x[4], const double a, const Exponent& b, 01707 const Exponent& c ) 01708 { 01709 static Vector3D my_x[4]; 01710 01711 my_x[0] = x[0]; 01712 my_x[1] = x[2]; 01713 my_x[2] = x[3]; 01714 my_x[3] = x[1]; 01715 return g_fcn_3e_v3( obj, g_obj, my_x, a, b, c ); 01716 } 01717 01718 inline bool g_fcn_3e_v2( double& obj, Vector3D& g_obj, const Vector3D x[4], const double a, const Exponent& b, 01719 const Exponent& c ) 01720 { 01721 static Vector3D my_x[4]; 01722 01723 my_x[0] = x[1]; 01724 my_x[1] = x[0]; 01725 my_x[2] = x[3]; 01726 my_x[3] = x[2]; 01727 return g_fcn_3e_v3( obj, g_obj, my_x, a, b, c ); 01728 } 01729 01730 inline bool h_fcn_3e_v3( double& obj, Vector3D& g_obj, Matrix3D& h_obj, const Vector3D x[4], const double a, 01731 const Exponent& b, const Exponent& c ) 01732 { 01733 double matr[9], f, g; 01734 double dg[9], loc0, /*loc1,*/ loc3, loc4; 01735 double cross; 01736 01737 /* Calculate M = A*inv(W). */ 01738 f = x[1][0] + x[0][0]; 01739 matr[0] = x[1][0] - x[0][0]; 01740 matr[1] = ( 2.0 * x[2][0] - f ) * isqrt3; 01741 matr[2] = ( 3.0 * x[3][0] - x[2][0] - f ) * isqrt6; 01742 01743 f = x[1][1] + x[0][1]; 01744 matr[3] = x[1][1] - x[0][1]; 01745 matr[4] = ( 2.0 * x[2][1] - f ) * isqrt3; 01746 matr[5] = ( 3.0 * x[3][1] - x[2][1] - f ) * isqrt6; 01747 01748 f = x[1][2] + x[0][2]; 01749 matr[6] = x[1][2] - x[0][2]; 01750 matr[7] = ( 2.0 * x[2][2] - f ) * isqrt3; 01751 matr[8] = ( 3.0 * x[3][2] - x[2][2] - f ) * isqrt6; 01752 01753 /* Calculate det(M). */ 01754 dg[0] = matr[4] * matr[8] - matr[5] * matr[7]; 01755 dg[1] = matr[5] * matr[6] - matr[3] * matr[8]; 01756 dg[2] = matr[3] * matr[7] - matr[4] * matr[6]; 01757 g = matr[0] * dg[0] + matr[1] * dg[1] + matr[2] * dg[2]; 01758 if( g < MSQ_MIN ) 01759 { 01760 obj = g; 01761 return false; 01762 } 01763 01764 /* Calculate norm(M). */ 01765 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 01766 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 01767 01768 loc3 = f; 01769 loc4 = g; 01770 01771 /* Calculate objective function. */ 01772 obj = a * b.raise( f ) * c.raise( g ); 01773 01774 /* Calculate the derivative of the objective function. */ 01775 f = b.value() * obj / f; /* Constant on nabla f */ 01776 g = c.value() * obj / g; /* Constant on nable g */ 01777 f *= 2.0; /* Modification for nabla f */ 01778 01779 dg[5] = matr[1] * matr[6] - matr[0] * matr[7]; 01780 dg[8] = matr[0] * matr[4] - matr[1] * matr[3]; 01781 01782 g_obj[0] = tisqrt6 * ( matr[2] * f + dg[2] * g ); 01783 g_obj[1] = tisqrt6 * ( matr[5] * f + dg[5] * g ); 01784 g_obj[2] = tisqrt6 * ( matr[8] * f + dg[8] * g ); 01785 01786 /* Calculate the hessian of the objective. */ 01787 loc0 = f; /* Constant on nabla^2 f */ 01788 // loc1 = g; /* Constant on nabla^2 g */ 01789 cross = f * c.value() / loc4; /* Constant on nabla g nabla f */ 01790 f = f * ( b.value() - 1 ) / loc3; /* Constant on nabla f nabla f */ 01791 g = g * ( c.value() - 1 ) / loc4; /* Constant on nabla g nabla g */ 01792 f *= 2.0; /* Modification for nabla f */ 01793 01794 /* First block of rows */ 01795 loc3 = matr[2] * f + dg[2] * cross; 01796 loc4 = dg[2] * g + matr[2] * cross; 01797 01798 h_obj[0][0] = 1.5 * ( loc0 + loc3 * matr[2] + loc4 * dg[2] ); 01799 h_obj[0][1] = 1.5 * ( loc3 * matr[5] + loc4 * dg[5] ); 01800 h_obj[0][2] = 1.5 * ( loc3 * matr[8] + loc4 * dg[8] ); 01801 01802 /* Second block of rows */ 01803 loc3 = matr[5] * f + dg[5] * cross; 01804 loc4 = dg[5] * g + matr[5] * cross; 01805 01806 h_obj[1][1] = 1.5 * ( loc0 + loc3 * matr[5] + loc4 * dg[5] ); 01807 h_obj[1][2] = 1.5 * ( loc3 * matr[8] + loc4 * dg[8] ); 01808 01809 /* Third block of rows */ 01810 loc3 = matr[8] * f + dg[8] * cross; 01811 loc4 = dg[8] * g + matr[8] * cross; 01812 01813 h_obj[2][2] = 1.5 * ( loc0 + loc3 * matr[8] + loc4 * dg[8] ); 01814 01815 // completes diagonal block. 01816 h_obj.fill_lower_triangle(); 01817 return true; 01818 } 01819 01820 inline bool h_fcn_3e_v0( double& obj, Vector3D& g_obj, Matrix3D& h_obj, const Vector3D x[4], const double a, 01821 const Exponent& b, const Exponent& c ) 01822 { 01823 static Vector3D my_x[4]; 01824 01825 my_x[0] = x[1]; 01826 my_x[1] = x[3]; 01827 my_x[2] = x[2]; 01828 my_x[3] = x[0]; 01829 return h_fcn_3e_v3( obj, g_obj, h_obj, my_x, a, b, c ); 01830 } 01831 01832 inline bool h_fcn_3e_v1( double& obj, Vector3D& g_obj, Matrix3D& h_obj, const Vector3D x[4], const double a, 01833 const Exponent& b, const Exponent& c ) 01834 { 01835 static Vector3D my_x[4]; 01836 01837 my_x[0] = x[0]; 01838 my_x[1] = x[2]; 01839 my_x[2] = x[3]; 01840 my_x[3] = x[1]; 01841 return h_fcn_3e_v3( obj, g_obj, h_obj, my_x, a, b, c ); 01842 } 01843 01844 inline bool h_fcn_3e_v2( double& obj, Vector3D& g_obj, Matrix3D& h_obj, const Vector3D x[4], const double a, 01845 const Exponent& b, const Exponent& c ) 01846 { 01847 static Vector3D my_x[4]; 01848 01849 my_x[0] = x[1]; 01850 my_x[1] = x[0]; 01851 my_x[2] = x[3]; 01852 my_x[3] = x[2]; 01853 return h_fcn_3e_v3( obj, g_obj, h_obj, my_x, a, b, c ); 01854 } 01855 01856 /*****************************************************************************/ 01857 /* The following set of functions reference tetrahedral elements to a */ 01858 /* right tetrahedron. They are used when assessing the quality of a */ 01859 /* hexahedral element. A zero return value indicates success, while */ 01860 /* a nonzero value indicates failure. */ 01861 /*****************************************************************************/ 01862 01863 /*****************************************************************************/ 01864 /* Function evaluation requires 53 flops. */ 01865 /* Reductions possible when b == 1, c == 1, or d == 1 */ 01866 /*****************************************************************************/ 01867 inline bool m_fcn_3i( double& obj, const Vector3D x[4], const double a, const Exponent& b, const Exponent& c, 01868 const Vector3D& d ) 01869 { 01870 double matr[9], f; 01871 double g; 01872 01873 /* Calculate M = A*inv(W). */ 01874 matr[0] = d[0] * ( x[1][0] - x[0][0] ); 01875 matr[1] = d[1] * ( x[2][0] - x[0][0] ); 01876 matr[2] = d[2] * ( x[3][0] - x[0][0] ); 01877 01878 matr[3] = d[0] * ( x[1][1] - x[0][1] ); 01879 matr[4] = d[1] * ( x[2][1] - x[0][1] ); 01880 matr[5] = d[2] * ( x[3][1] - x[0][1] ); 01881 01882 matr[6] = d[0] * ( x[1][2] - x[0][2] ); 01883 matr[7] = d[1] * ( x[2][2] - x[0][2] ); 01884 matr[8] = d[2] * ( x[3][2] - x[0][2] ); 01885 01886 /* Calculate det(M). */ 01887 g = matr[0] * ( matr[4] * matr[8] - matr[5] * matr[7] ) + matr[1] * ( matr[5] * matr[6] - matr[3] * matr[8] ) + 01888 matr[2] * ( matr[3] * matr[7] - matr[4] * matr[6] ); 01889 if( g < MSQ_MIN ) 01890 { 01891 obj = g; 01892 return false; 01893 } 01894 01895 /* Calculate norm(M). */ 01896 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 01897 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 01898 01899 /* Calculate objective function. */ 01900 obj = a * b.raise( f ) * c.raise( g ); 01901 return true; 01902 } 01903 01904 /*****************************************************************************/ 01905 /* Gradient evaluation requires 115 flops. */ 01906 /* Reductions possible when b == 1, c == 1, or d == 1 */ 01907 /*****************************************************************************/ 01908 inline bool g_fcn_3i( double& obj, Vector3D g_obj[4], const Vector3D x[4], const double a, const Exponent& b, 01909 const Exponent& c, const Vector3D& d ) 01910 { 01911 double matr[9], f; 01912 double adj_m[9], g; 01913 double loc1, loc2, loc3; 01914 01915 /* Calculate M = A*inv(W). */ 01916 matr[0] = d[0] * ( x[1][0] - x[0][0] ); 01917 matr[1] = d[1] * ( x[2][0] - x[0][0] ); 01918 matr[2] = d[2] * ( x[3][0] - x[0][0] ); 01919 01920 matr[3] = d[0] * ( x[1][1] - x[0][1] ); 01921 matr[4] = d[1] * ( x[2][1] - x[0][1] ); 01922 matr[5] = d[2] * ( x[3][1] - x[0][1] ); 01923 01924 matr[6] = d[0] * ( x[1][2] - x[0][2] ); 01925 matr[7] = d[1] * ( x[2][2] - x[0][2] ); 01926 matr[8] = d[2] * ( x[3][2] - x[0][2] ); 01927 01928 /* Calculate det(M). */ 01929 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 01930 loc2 = matr[5] * matr[6] - matr[3] * matr[8]; 01931 loc3 = matr[3] * matr[7] - matr[4] * matr[6]; 01932 g = matr[0] * loc1 + matr[1] * loc2 + matr[2] * loc3; 01933 if( g < MSQ_MIN ) 01934 { 01935 obj = g; 01936 return false; 01937 } 01938 01939 /* Calculate norm(M). */ 01940 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 01941 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 01942 01943 /* Calculate objective function. */ 01944 obj = a * b.raise( f ) * c.raise( g ); 01945 01946 /* Calculate the derivative of the objective function. */ 01947 f = b.value() * obj / f; /* Constant on nabla f */ 01948 g = c.value() * obj / g; /* Constant on nable g */ 01949 f *= 2.0; /* Modification for nabla f */ 01950 01951 adj_m[0] = d[0] * ( matr[0] * f + loc1 * g ); 01952 adj_m[1] = d[1] * ( matr[1] * f + loc2 * g ); 01953 adj_m[2] = d[2] * ( matr[2] * f + loc3 * g ); 01954 01955 loc1 = matr[0] * g; 01956 loc2 = matr[1] * g; 01957 loc3 = matr[2] * g; 01958 01959 adj_m[3] = d[0] * ( matr[3] * f + loc3 * matr[7] - loc2 * matr[8] ); 01960 adj_m[4] = d[1] * ( matr[4] * f + loc1 * matr[8] - loc3 * matr[6] ); 01961 adj_m[5] = d[2] * ( matr[5] * f + loc2 * matr[6] - loc1 * matr[7] ); 01962 01963 adj_m[6] = d[0] * ( matr[6] * f + loc2 * matr[5] - loc3 * matr[4] ); 01964 adj_m[7] = d[1] * ( matr[7] * f + loc3 * matr[3] - loc1 * matr[5] ); 01965 adj_m[8] = d[2] * ( matr[8] * f + loc1 * matr[4] - loc2 * matr[3] ); 01966 01967 g_obj[0][0] = -adj_m[0] - adj_m[1] - adj_m[2]; 01968 g_obj[1][0] = adj_m[0]; 01969 g_obj[2][0] = adj_m[1]; 01970 g_obj[3][0] = adj_m[2]; 01971 01972 g_obj[0][1] = -adj_m[3] - adj_m[4] - adj_m[5]; 01973 g_obj[1][1] = adj_m[3]; 01974 g_obj[2][1] = adj_m[4]; 01975 g_obj[3][1] = adj_m[5]; 01976 01977 g_obj[0][2] = -adj_m[6] - adj_m[7] - adj_m[8]; 01978 g_obj[1][2] = adj_m[6]; 01979 g_obj[2][2] = adj_m[7]; 01980 g_obj[3][2] = adj_m[8]; 01981 return true; 01982 } 01983 01984 /*****************************************************************************/ 01985 /* Hessian evaluation requires 469 flops. */ 01986 /* Reductions possible when b == 1, c == 1, or d == 1 */ 01987 /*****************************************************************************/ 01988 inline int h_fcn_3i( double& obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const double a, 01989 const Exponent& b, const Exponent& c, const Vector3D& d ) 01990 { 01991 double matr[9], f; 01992 double adj_m[9], g; 01993 double dg[9], loc0, loc1, loc2, loc3, loc4; 01994 double A[3], J_A[6], J_B[9], J_C[9], cross; 01995 01996 const double scale[6] = { d[0] * d[0], d[0] * d[1], d[0] * d[2], d[1] * d[1], d[1] * d[2], d[2] * d[2] }; 01997 01998 /* Calculate M = A*inv(W). */ 01999 matr[0] = d[0] * ( x[1][0] - x[0][0] ); 02000 matr[1] = d[1] * ( x[2][0] - x[0][0] ); 02001 matr[2] = d[2] * ( x[3][0] - x[0][0] ); 02002 02003 matr[3] = d[0] * ( x[1][1] - x[0][1] ); 02004 matr[4] = d[1] * ( x[2][1] - x[0][1] ); 02005 matr[5] = d[2] * ( x[3][1] - x[0][1] ); 02006 02007 matr[6] = d[0] * ( x[1][2] - x[0][2] ); 02008 matr[7] = d[1] * ( x[2][2] - x[0][2] ); 02009 matr[8] = d[2] * ( x[3][2] - x[0][2] ); 02010 02011 /* Calculate det(M). */ 02012 dg[0] = matr[4] * matr[8] - matr[5] * matr[7]; 02013 dg[1] = matr[5] * matr[6] - matr[3] * matr[8]; 02014 dg[2] = matr[3] * matr[7] - matr[4] * matr[6]; 02015 g = matr[0] * dg[0] + matr[1] * dg[1] + matr[2] * dg[2]; 02016 if( g < MSQ_MIN ) 02017 { 02018 obj = g; 02019 return false; 02020 } 02021 02022 /* Calculate norm(M). */ 02023 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 02024 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 02025 02026 loc3 = f; 02027 loc4 = g; 02028 02029 /* Calculate objective function. */ 02030 obj = a * b.raise( f ) * c.raise( g ); 02031 02032 /* Calculate the derivative of the objective function. */ 02033 f = b.value() * obj / f; /* Constant on nabla f */ 02034 g = c.value() * obj / g; /* Constant on nable g */ 02035 f *= 2.0; /* Modification for nabla f */ 02036 02037 dg[3] = matr[2] * matr[7] - matr[1] * matr[8]; 02038 dg[4] = matr[0] * matr[8] - matr[2] * matr[6]; 02039 dg[5] = matr[1] * matr[6] - matr[0] * matr[7]; 02040 dg[6] = matr[1] * matr[5] - matr[2] * matr[4]; 02041 dg[7] = matr[2] * matr[3] - matr[0] * matr[5]; 02042 dg[8] = matr[0] * matr[4] - matr[1] * matr[3]; 02043 02044 adj_m[0] = d[0] * ( matr[0] * f + dg[0] * g ); 02045 adj_m[1] = d[1] * ( matr[1] * f + dg[1] * g ); 02046 adj_m[2] = d[2] * ( matr[2] * f + dg[2] * g ); 02047 adj_m[3] = d[0] * ( matr[3] * f + dg[3] * g ); 02048 adj_m[4] = d[1] * ( matr[4] * f + dg[4] * g ); 02049 adj_m[5] = d[2] * ( matr[5] * f + dg[5] * g ); 02050 adj_m[6] = d[0] * ( matr[6] * f + dg[6] * g ); 02051 adj_m[7] = d[1] * ( matr[7] * f + dg[7] * g ); 02052 adj_m[8] = d[2] * ( matr[8] * f + dg[8] * g ); 02053 02054 g_obj[0][0] = -adj_m[0] - adj_m[1] - adj_m[2]; 02055 g_obj[1][0] = adj_m[0]; 02056 g_obj[2][0] = adj_m[1]; 02057 g_obj[3][0] = adj_m[2]; 02058 02059 g_obj[0][1] = -adj_m[3] - adj_m[4] - adj_m[5]; 02060 g_obj[1][1] = adj_m[3]; 02061 g_obj[2][1] = adj_m[4]; 02062 g_obj[3][1] = adj_m[5]; 02063 02064 g_obj[0][2] = -adj_m[6] - adj_m[7] - adj_m[8]; 02065 g_obj[1][2] = adj_m[6]; 02066 g_obj[2][2] = adj_m[7]; 02067 g_obj[3][2] = adj_m[8]; 02068 02069 /* Calculate the hessian of the objective. */ 02070 loc0 = f; /* Constant on nabla^2 f */ 02071 loc1 = g; /* Constant on nabla^2 g */ 02072 cross = f * c.value() / loc4; /* Constant on nabla g nabla f */ 02073 f = f * ( b.value() - 1 ) / loc3; /* Constant on nabla f nabla f */ 02074 g = g * ( c.value() - 1 ) / loc4; /* Constant on nabla g nabla g */ 02075 f *= 2.0; /* Modification for nabla f */ 02076 02077 /* First block of rows */ 02078 loc3 = matr[0] * f + dg[0] * cross; 02079 loc4 = dg[0] * g + matr[0] * cross; 02080 02081 J_A[0] = loc0 + loc3 * matr[0] + loc4 * dg[0]; 02082 J_A[1] = loc3 * matr[1] + loc4 * dg[1]; 02083 J_A[2] = loc3 * matr[2] + loc4 * dg[2]; 02084 J_B[0] = loc3 * matr[3] + loc4 * dg[3]; 02085 J_B[1] = loc3 * matr[4] + loc4 * dg[4]; 02086 J_B[2] = loc3 * matr[5] + loc4 * dg[5]; 02087 J_C[0] = loc3 * matr[6] + loc4 * dg[6]; 02088 J_C[1] = loc3 * matr[7] + loc4 * dg[7]; 02089 J_C[2] = loc3 * matr[8] + loc4 * dg[8]; 02090 02091 loc3 = matr[1] * f + dg[1] * cross; 02092 loc4 = dg[1] * g + matr[1] * cross; 02093 02094 J_A[3] = loc0 + loc3 * matr[1] + loc4 * dg[1]; 02095 J_A[4] = loc3 * matr[2] + loc4 * dg[2]; 02096 J_B[3] = loc3 * matr[3] + loc4 * dg[3]; 02097 J_B[4] = loc3 * matr[4] + loc4 * dg[4]; 02098 J_B[5] = loc3 * matr[5] + loc4 * dg[5]; 02099 J_C[3] = loc3 * matr[6] + loc4 * dg[6]; 02100 J_C[4] = loc3 * matr[7] + loc4 * dg[7]; 02101 J_C[5] = loc3 * matr[8] + loc4 * dg[8]; 02102 02103 loc3 = matr[2] * f + dg[2] * cross; 02104 loc4 = dg[2] * g + matr[2] * cross; 02105 02106 J_A[5] = loc0 + loc3 * matr[2] + loc4 * dg[2]; 02107 J_B[6] = loc3 * matr[3] + loc4 * dg[3]; 02108 J_B[7] = loc3 * matr[4] + loc4 * dg[4]; 02109 J_B[8] = loc3 * matr[5] + loc4 * dg[5]; 02110 J_C[6] = loc3 * matr[6] + loc4 * dg[6]; 02111 J_C[7] = loc3 * matr[7] + loc4 * dg[7]; 02112 J_C[8] = loc3 * matr[8] + loc4 * dg[8]; 02113 02114 /* First diagonal block */ 02115 J_A[0] *= scale[0]; 02116 J_A[1] *= scale[1]; 02117 J_A[2] *= scale[2]; 02118 J_A[3] *= scale[3]; 02119 J_A[4] *= scale[4]; 02120 J_A[5] *= scale[5]; 02121 02122 A[0] = -J_A[0] - J_A[1] - J_A[2]; 02123 A[1] = -J_A[1] - J_A[3] - J_A[4]; 02124 A[2] = -J_A[2] - J_A[4] - J_A[5]; 02125 02126 h_obj[0][0][0] = -A[0] - A[1] - A[2]; 02127 h_obj[1][0][0] = A[0]; 02128 h_obj[2][0][0] = A[1]; 02129 h_obj[3][0][0] = A[2]; 02130 02131 h_obj[4][0][0] = J_A[0]; 02132 h_obj[5][0][0] = J_A[1]; 02133 h_obj[6][0][0] = J_A[2]; 02134 02135 h_obj[7][0][0] = J_A[3]; 02136 h_obj[8][0][0] = J_A[4]; 02137 02138 h_obj[9][0][0] = J_A[5]; 02139 02140 /* First off-diagonal block */ 02141 loc2 = matr[8] * loc1; 02142 J_B[1] += loc2; 02143 J_B[3] -= loc2; 02144 02145 loc2 = matr[7] * loc1; 02146 J_B[2] -= loc2; 02147 J_B[6] += loc2; 02148 02149 loc2 = matr[6] * loc1; 02150 J_B[5] += loc2; 02151 J_B[7] -= loc2; 02152 02153 J_B[0] *= scale[0]; 02154 J_B[1] *= scale[1]; 02155 J_B[2] *= scale[2]; 02156 J_B[3] *= scale[1]; 02157 J_B[4] *= scale[3]; 02158 J_B[5] *= scale[4]; 02159 J_B[6] *= scale[2]; 02160 J_B[7] *= scale[4]; 02161 J_B[8] *= scale[5]; 02162 02163 A[0] = -J_B[0] - J_B[3] - J_B[6]; 02164 A[1] = -J_B[1] - J_B[4] - J_B[7]; 02165 A[2] = -J_B[2] - J_B[5] - J_B[8]; 02166 02167 h_obj[0][0][1] = -A[0] - A[1] - A[2]; 02168 h_obj[1][0][1] = A[0]; 02169 h_obj[2][0][1] = A[1]; 02170 h_obj[3][0][1] = A[2]; 02171 02172 h_obj[1][1][0] = -J_B[0] - J_B[1] - J_B[2]; 02173 h_obj[4][0][1] = J_B[0]; 02174 h_obj[5][0][1] = J_B[1]; 02175 h_obj[6][0][1] = J_B[2]; 02176 02177 h_obj[2][1][0] = -J_B[3] - J_B[4] - J_B[5]; 02178 h_obj[5][1][0] = J_B[3]; 02179 h_obj[7][0][1] = J_B[4]; 02180 h_obj[8][0][1] = J_B[5]; 02181 02182 h_obj[3][1][0] = -J_B[6] - J_B[7] - J_B[8]; 02183 h_obj[6][1][0] = J_B[6]; 02184 h_obj[8][1][0] = J_B[7]; 02185 h_obj[9][0][1] = J_B[8]; 02186 02187 /* Second off-diagonal block */ 02188 loc2 = matr[5] * loc1; 02189 J_C[1] -= loc2; 02190 J_C[3] += loc2; 02191 02192 loc2 = matr[4] * loc1; 02193 J_C[2] += loc2; 02194 J_C[6] -= loc2; 02195 02196 loc2 = matr[3] * loc1; 02197 J_C[5] -= loc2; 02198 J_C[7] += loc2; 02199 02200 J_C[0] *= scale[0]; 02201 J_C[1] *= scale[1]; 02202 J_C[2] *= scale[2]; 02203 J_C[3] *= scale[1]; 02204 J_C[4] *= scale[3]; 02205 J_C[5] *= scale[4]; 02206 J_C[6] *= scale[2]; 02207 J_C[7] *= scale[4]; 02208 J_C[8] *= scale[5]; 02209 02210 A[0] = -J_C[0] - J_C[3] - J_C[6]; 02211 A[1] = -J_C[1] - J_C[4] - J_C[7]; 02212 A[2] = -J_C[2] - J_C[5] - J_C[8]; 02213 02214 h_obj[0][0][2] = -A[0] - A[1] - A[2]; 02215 h_obj[1][0][2] = A[0]; 02216 h_obj[2][0][2] = A[1]; 02217 h_obj[3][0][2] = A[2]; 02218 02219 h_obj[1][2][0] = -J_C[0] - J_C[1] - J_C[2]; 02220 h_obj[4][0][2] = J_C[0]; 02221 h_obj[5][0][2] = J_C[1]; 02222 h_obj[6][0][2] = J_C[2]; 02223 02224 h_obj[2][2][0] = -J_C[3] - J_C[4] - J_C[5]; 02225 h_obj[5][2][0] = J_C[3]; 02226 h_obj[7][0][2] = J_C[4]; 02227 h_obj[8][0][2] = J_C[5]; 02228 02229 h_obj[3][2][0] = -J_C[6] - J_C[7] - J_C[8]; 02230 h_obj[6][2][0] = J_C[6]; 02231 h_obj[8][2][0] = J_C[7]; 02232 h_obj[9][0][2] = J_C[8]; 02233 02234 /* Second block of rows */ 02235 loc3 = matr[3] * f + dg[3] * cross; 02236 loc4 = dg[3] * g + matr[3] * cross; 02237 02238 J_A[0] = loc0 + loc3 * matr[3] + loc4 * dg[3]; 02239 J_A[1] = loc3 * matr[4] + loc4 * dg[4]; 02240 J_A[2] = loc3 * matr[5] + loc4 * dg[5]; 02241 J_B[0] = loc3 * matr[6] + loc4 * dg[6]; 02242 J_B[1] = loc3 * matr[7] + loc4 * dg[7]; 02243 J_B[2] = loc3 * matr[8] + loc4 * dg[8]; 02244 02245 loc3 = matr[4] * f + dg[4] * cross; 02246 loc4 = dg[4] * g + matr[4] * cross; 02247 02248 J_A[3] = loc0 + loc3 * matr[4] + loc4 * dg[4]; 02249 J_A[4] = loc3 * matr[5] + loc4 * dg[5]; 02250 J_B[3] = loc3 * matr[6] + loc4 * dg[6]; 02251 J_B[4] = loc3 * matr[7] + loc4 * dg[7]; 02252 J_B[5] = loc3 * matr[8] + loc4 * dg[8]; 02253 02254 loc3 = matr[5] * f + dg[5] * cross; 02255 loc4 = dg[5] * g + matr[5] * cross; 02256 02257 J_A[5] = loc0 + loc3 * matr[5] + loc4 * dg[5]; 02258 J_B[6] = loc3 * matr[6] + loc4 * dg[6]; 02259 J_B[7] = loc3 * matr[7] + loc4 * dg[7]; 02260 J_B[8] = loc3 * matr[8] + loc4 * dg[8]; 02261 02262 /* Second diagonal block */ 02263 J_A[0] *= scale[0]; 02264 J_A[1] *= scale[1]; 02265 J_A[2] *= scale[2]; 02266 J_A[3] *= scale[3]; 02267 J_A[4] *= scale[4]; 02268 J_A[5] *= scale[5]; 02269 02270 A[0] = -J_A[0] - J_A[1] - J_A[2]; 02271 A[1] = -J_A[1] - J_A[3] - J_A[4]; 02272 A[2] = -J_A[2] - J_A[4] - J_A[5]; 02273 02274 h_obj[0][1][1] = -A[0] - A[1] - A[2]; 02275 h_obj[1][1][1] = A[0]; 02276 h_obj[2][1][1] = A[1]; 02277 h_obj[3][1][1] = A[2]; 02278 02279 h_obj[4][1][1] = J_A[0]; 02280 h_obj[5][1][1] = J_A[1]; 02281 h_obj[6][1][1] = J_A[2]; 02282 02283 h_obj[7][1][1] = J_A[3]; 02284 h_obj[8][1][1] = J_A[4]; 02285 02286 h_obj[9][1][1] = J_A[5]; 02287 02288 /* Third off-diagonal block */ 02289 loc2 = matr[2] * loc1; 02290 J_B[1] += loc2; 02291 J_B[3] -= loc2; 02292 02293 loc2 = matr[1] * loc1; 02294 J_B[2] -= loc2; 02295 J_B[6] += loc2; 02296 02297 loc2 = matr[0] * loc1; 02298 J_B[5] += loc2; 02299 J_B[7] -= loc2; 02300 02301 J_B[0] *= scale[0]; 02302 J_B[1] *= scale[1]; 02303 J_B[2] *= scale[2]; 02304 J_B[3] *= scale[1]; 02305 J_B[4] *= scale[3]; 02306 J_B[5] *= scale[4]; 02307 J_B[6] *= scale[2]; 02308 J_B[7] *= scale[4]; 02309 J_B[8] *= scale[5]; 02310 02311 A[0] = -J_B[0] - J_B[3] - J_B[6]; 02312 A[1] = -J_B[1] - J_B[4] - J_B[7]; 02313 A[2] = -J_B[2] - J_B[5] - J_B[8]; 02314 02315 h_obj[0][1][2] = -A[0] - A[1] - A[2]; 02316 h_obj[1][1][2] = A[0]; 02317 h_obj[2][1][2] = A[1]; 02318 h_obj[3][1][2] = A[2]; 02319 02320 h_obj[1][2][1] = -J_B[0] - J_B[1] - J_B[2]; 02321 h_obj[4][1][2] = J_B[0]; 02322 h_obj[5][1][2] = J_B[1]; 02323 h_obj[6][1][2] = J_B[2]; 02324 02325 h_obj[2][2][1] = -J_B[3] - J_B[4] - J_B[5]; 02326 h_obj[5][2][1] = J_B[3]; 02327 h_obj[7][1][2] = J_B[4]; 02328 h_obj[8][1][2] = J_B[5]; 02329 02330 h_obj[3][2][1] = -J_B[6] - J_B[7] - J_B[8]; 02331 h_obj[6][2][1] = J_B[6]; 02332 h_obj[8][2][1] = J_B[7]; 02333 h_obj[9][1][2] = J_B[8]; 02334 02335 /* Third block of rows */ 02336 loc3 = matr[6] * f + dg[6] * cross; 02337 loc4 = dg[6] * g + matr[6] * cross; 02338 02339 J_A[0] = loc0 + loc3 * matr[6] + loc4 * dg[6]; 02340 J_A[1] = loc3 * matr[7] + loc4 * dg[7]; 02341 J_A[2] = loc3 * matr[8] + loc4 * dg[8]; 02342 02343 loc3 = matr[7] * f + dg[7] * cross; 02344 loc4 = dg[7] * g + matr[7] * cross; 02345 02346 J_A[3] = loc0 + loc3 * matr[7] + loc4 * dg[7]; 02347 J_A[4] = loc3 * matr[8] + loc4 * dg[8]; 02348 02349 loc3 = matr[8] * f + dg[8] * cross; 02350 loc4 = dg[8] * g + matr[8] * cross; 02351 02352 J_A[5] = loc0 + loc3 * matr[8] + loc4 * dg[8]; 02353 02354 /* Third diagonal block */ 02355 J_A[0] *= scale[0]; 02356 J_A[1] *= scale[1]; 02357 J_A[2] *= scale[2]; 02358 J_A[3] *= scale[3]; 02359 J_A[4] *= scale[4]; 02360 J_A[5] *= scale[5]; 02361 02362 A[0] = -J_A[0] - J_A[1] - J_A[2]; 02363 A[1] = -J_A[1] - J_A[3] - J_A[4]; 02364 A[2] = -J_A[2] - J_A[4] - J_A[5]; 02365 02366 h_obj[0][2][2] = -A[0] - A[1] - A[2]; 02367 h_obj[1][2][2] = A[0]; 02368 h_obj[2][2][2] = A[1]; 02369 h_obj[3][2][2] = A[2]; 02370 02371 h_obj[4][2][2] = J_A[0]; 02372 h_obj[5][2][2] = J_A[1]; 02373 h_obj[6][2][2] = J_A[2]; 02374 02375 h_obj[7][2][2] = J_A[3]; 02376 h_obj[8][2][2] = J_A[4]; 02377 02378 h_obj[9][2][2] = J_A[5]; 02379 02380 // completes diagonal blocks. 02381 h_obj[0].fill_lower_triangle(); 02382 h_obj[4].fill_lower_triangle(); 02383 h_obj[7].fill_lower_triangle(); 02384 h_obj[9].fill_lower_triangle(); 02385 return true; 02386 } 02387 02388 /********************************************************************* 02389 * Reference tetrahedral elements to halves of an ideal pyramid. 02390 * Vertices should be ordered such that the edge between the 2nd and 02391 * 3rd vertices is the one longer edge in the reference tetrahedron 02392 * (the diagonal of the base of the pyramid of which the tetrahedron 02393 * is one half of). 02394 * 1 0 1/2 1 0 -1/sqrt(2) 02395 * W = 0 1 1/2 inv(W) = 0 1 -1/sqrt(2) 02396 * 0 0 1/sqrt(2) 0 0 sqrt(2) 02397 * 02398 *********************************************************************/ 02399 inline bool m_fcn_3p( double& obj, const Vector3D x[4], const double a, const Exponent& b, const Exponent& c ) 02400 { 02401 const double h = 0.5; /* h = 1 / (2*height) */ 02402 02403 double matr[9], f; 02404 double g; 02405 02406 /* Calculate M = A*inv(W). */ 02407 matr[0] = x[1][0] - x[0][0]; 02408 matr[1] = x[2][0] - x[0][0]; 02409 matr[2] = ( 2.0 * x[3][0] - x[1][0] - x[2][0] ) * h; 02410 02411 matr[3] = x[1][1] - x[0][1]; 02412 matr[4] = x[2][1] - x[0][1]; 02413 matr[5] = ( 2.0 * x[3][1] - x[1][1] - x[2][1] ) * h; 02414 02415 matr[6] = x[1][2] - x[0][2]; 02416 matr[7] = x[2][2] - x[0][2]; 02417 matr[8] = ( 2.0 * x[3][2] - x[1][2] - x[2][2] ) * h; 02418 02419 /* Calculate det(M). */ 02420 g = matr[0] * ( matr[4] * matr[8] - matr[5] * matr[7] ) + matr[1] * ( matr[5] * matr[6] - matr[3] * matr[8] ) + 02421 matr[2] * ( matr[3] * matr[7] - matr[4] * matr[6] ); 02422 if( g < MSQ_MIN ) 02423 { 02424 obj = g; 02425 return false; 02426 } 02427 02428 /* Calculate norm(M). */ 02429 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 02430 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 02431 02432 /* Calculate objective function. */ 02433 obj = a * b.raise( f ) * c.raise( g ); 02434 return true; 02435 } 02436 02437 inline bool g_fcn_3p( double& obj, Vector3D g_obj[4], const Vector3D x[4], const double a, const Exponent& b, 02438 const Exponent& c ) 02439 { 02440 const double h = 0.5; /* h = 1 / (2*height) */ 02441 const double th = 1.0; /* h = 1 / (height) */ 02442 02443 double matr[9], f; 02444 double adj_m[9], g; 02445 double loc1, loc2, loc3; 02446 02447 /* Calculate M = A*inv(W). */ 02448 matr[0] = x[1][0] - x[0][0]; 02449 matr[1] = x[2][0] - x[0][0]; 02450 matr[2] = ( 2.0 * x[3][0] - x[1][0] - x[2][0] ) * h; 02451 02452 matr[3] = x[1][1] - x[0][1]; 02453 matr[4] = x[2][1] - x[0][1]; 02454 matr[5] = ( 2.0 * x[3][1] - x[1][1] - x[2][1] ) * h; 02455 02456 matr[6] = x[1][2] - x[0][2]; 02457 matr[7] = x[2][2] - x[0][2]; 02458 matr[8] = ( 2.0 * x[3][2] - x[1][2] - x[2][2] ) * h; 02459 02460 /* Calculate det(M). */ 02461 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 02462 loc2 = matr[5] * matr[6] - matr[3] * matr[8]; 02463 loc3 = matr[3] * matr[7] - matr[4] * matr[6]; 02464 g = matr[0] * loc1 + matr[1] * loc2 + matr[2] * loc3; 02465 if( g < MSQ_MIN ) 02466 { 02467 obj = g; 02468 return false; 02469 } 02470 02471 /* Calculate norm(M). */ 02472 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 02473 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 02474 02475 /* Calculate objective function. */ 02476 obj = a * b.raise( f ) * c.raise( g ); 02477 02478 /* Calculate the derivative of the objective function. */ 02479 f = b.value() * obj / f; /* Constant on nabla f */ 02480 g = c.value() * obj / g; /* Constant on nable g */ 02481 f *= 2.0; /* Modification for nabla f */ 02482 02483 adj_m[0] = matr[0] * f + loc1 * g; 02484 adj_m[1] = matr[1] * f + loc2 * g; 02485 adj_m[2] = matr[2] * f + loc3 * g; 02486 02487 loc1 = matr[0] * g; 02488 loc2 = matr[1] * g; 02489 loc3 = matr[2] * g; 02490 02491 adj_m[3] = matr[3] * f + loc3 * matr[7] - loc2 * matr[8]; 02492 adj_m[4] = matr[4] * f + loc1 * matr[8] - loc3 * matr[6]; 02493 adj_m[5] = matr[5] * f + loc2 * matr[6] - loc1 * matr[7]; 02494 02495 adj_m[6] = matr[6] * f + loc2 * matr[5] - loc3 * matr[4]; 02496 adj_m[7] = matr[7] * f + loc3 * matr[3] - loc1 * matr[5]; 02497 adj_m[8] = matr[8] * f + loc1 * matr[4] - loc2 * matr[3]; 02498 02499 g_obj[0][0] = -adj_m[0] - adj_m[1]; 02500 g_obj[1][0] = adj_m[0] - h * adj_m[2]; 02501 g_obj[2][0] = adj_m[1] - h * adj_m[2]; 02502 g_obj[3][0] = th * adj_m[2]; 02503 02504 g_obj[0][1] = -adj_m[3] - adj_m[4]; 02505 g_obj[1][1] = adj_m[3] - h * adj_m[5]; 02506 g_obj[2][1] = adj_m[4] - h * adj_m[5]; 02507 g_obj[3][1] = th * adj_m[5]; 02508 02509 g_obj[0][2] = -adj_m[6] - adj_m[7]; 02510 g_obj[1][2] = adj_m[6] - h * adj_m[8]; 02511 g_obj[2][2] = adj_m[7] - h * adj_m[8]; 02512 g_obj[3][2] = th * adj_m[8]; 02513 return true; 02514 } 02515 02516 inline bool h_fcn_3p( double& obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const double a, 02517 const Exponent& b, const Exponent& c ) 02518 { 02519 const double h = 0.5; /* h = 1 / (2*height) */ 02520 const double th = 1.0; /* h = 1 / (height) */ 02521 02522 double matr[9], f; 02523 double adj_m[9], g; 02524 double dg[9], loc0, loc1, loc2, loc3, loc4; 02525 double A[12], J_A[6], J_B[9], J_C[9], cross; 02526 02527 /* Calculate M = A*inv(W). */ 02528 matr[0] = x[1][0] - x[0][0]; 02529 matr[1] = x[2][0] - x[0][0]; 02530 matr[2] = ( 2.0 * x[3][0] - x[1][0] - x[2][0] ) * h; 02531 02532 matr[3] = x[1][1] - x[0][1]; 02533 matr[4] = x[2][1] - x[0][1]; 02534 matr[5] = ( 2.0 * x[3][1] - x[1][1] - x[2][1] ) * h; 02535 02536 matr[6] = x[1][2] - x[0][2]; 02537 matr[7] = x[2][2] - x[0][2]; 02538 matr[8] = ( 2.0 * x[3][2] - x[1][2] - x[2][2] ) * h; 02539 02540 /* Calculate det(M). */ 02541 dg[0] = matr[4] * matr[8] - matr[5] * matr[7]; 02542 dg[1] = matr[5] * matr[6] - matr[3] * matr[8]; 02543 dg[2] = matr[3] * matr[7] - matr[4] * matr[6]; 02544 g = matr[0] * dg[0] + matr[1] * dg[1] + matr[2] * dg[2]; 02545 if( g < MSQ_MIN ) 02546 { 02547 obj = g; 02548 return false; 02549 } 02550 02551 dg[3] = matr[2] * matr[7] - matr[1] * matr[8]; 02552 dg[4] = matr[0] * matr[8] - matr[2] * matr[6]; 02553 dg[5] = matr[1] * matr[6] - matr[0] * matr[7]; 02554 dg[6] = matr[1] * matr[5] - matr[2] * matr[4]; 02555 dg[7] = matr[2] * matr[3] - matr[0] * matr[5]; 02556 dg[8] = matr[0] * matr[4] - matr[1] * matr[3]; 02557 02558 /* Calculate norm(M). */ 02559 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 02560 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 02561 02562 loc3 = f; 02563 loc4 = g; 02564 02565 /* Calculate objective function. */ 02566 obj = a * b.raise( f ) * c.raise( g ); 02567 02568 /* Calculate the derivative of the objective function. */ 02569 f = b.value() * obj / f; /* Constant on nabla f */ 02570 g = c.value() * obj / g; /* Constant on nable g */ 02571 f *= 2.0; /* Modification for nabla f */ 02572 02573 adj_m[0] = matr[0] * f + dg[0] * g; 02574 adj_m[1] = matr[1] * f + dg[1] * g; 02575 adj_m[2] = matr[2] * f + dg[2] * g; 02576 adj_m[3] = matr[3] * f + dg[3] * g; 02577 adj_m[4] = matr[4] * f + dg[4] * g; 02578 adj_m[5] = matr[5] * f + dg[5] * g; 02579 adj_m[6] = matr[6] * f + dg[6] * g; 02580 adj_m[7] = matr[7] * f + dg[7] * g; 02581 adj_m[8] = matr[8] * f + dg[8] * g; 02582 02583 g_obj[0][0] = -adj_m[0] - adj_m[1]; 02584 g_obj[1][0] = adj_m[0] - h * adj_m[2]; 02585 g_obj[2][0] = adj_m[1] - h * adj_m[2]; 02586 g_obj[3][0] = th * adj_m[2]; 02587 02588 g_obj[0][1] = -adj_m[3] - adj_m[4]; 02589 g_obj[1][1] = adj_m[3] - h * adj_m[5]; 02590 g_obj[2][1] = adj_m[4] - h * adj_m[5]; 02591 g_obj[3][1] = th * adj_m[5]; 02592 02593 g_obj[0][2] = -adj_m[6] - adj_m[7]; 02594 g_obj[1][2] = adj_m[6] - h * adj_m[8]; 02595 g_obj[2][2] = adj_m[7] - h * adj_m[8]; 02596 g_obj[3][2] = th * adj_m[8]; 02597 02598 /* Calculate the hessian of the objective. */ 02599 loc0 = f; /* Constant on nabla^2 f */ 02600 loc1 = g; /* Constant on nabla^2 g */ 02601 cross = f * c.value() / loc4; /* Constant on nabla g nabla f */ 02602 f = f * ( b.value() - 1 ) / loc3; /* Constant on nabla f nabla f */ 02603 g = g * ( c.value() - 1 ) / loc4; /* Constant on nabla g nabla g */ 02604 f *= 2.0; /* Modification for nabla f */ 02605 02606 /* First block of rows */ 02607 loc3 = matr[0] * f + dg[0] * cross; 02608 loc4 = dg[0] * g + matr[0] * cross; 02609 02610 J_A[0] = loc0 + loc3 * matr[0] + loc4 * dg[0]; 02611 J_A[1] = loc3 * matr[1] + loc4 * dg[1]; 02612 J_A[2] = loc3 * matr[2] + loc4 * dg[2]; 02613 J_B[0] = loc3 * matr[3] + loc4 * dg[3]; 02614 J_B[1] = loc3 * matr[4] + loc4 * dg[4]; 02615 J_B[2] = loc3 * matr[5] + loc4 * dg[5]; 02616 J_C[0] = loc3 * matr[6] + loc4 * dg[6]; 02617 J_C[1] = loc3 * matr[7] + loc4 * dg[7]; 02618 J_C[2] = loc3 * matr[8] + loc4 * dg[8]; 02619 02620 loc3 = matr[1] * f + dg[1] * cross; 02621 loc4 = dg[1] * g + matr[1] * cross; 02622 02623 J_A[3] = loc0 + loc3 * matr[1] + loc4 * dg[1]; 02624 J_A[4] = loc3 * matr[2] + loc4 * dg[2]; 02625 J_B[3] = loc3 * matr[3] + loc4 * dg[3]; 02626 J_B[4] = loc3 * matr[4] + loc4 * dg[4]; 02627 J_B[5] = loc3 * matr[5] + loc4 * dg[5]; 02628 J_C[3] = loc3 * matr[6] + loc4 * dg[6]; 02629 J_C[4] = loc3 * matr[7] + loc4 * dg[7]; 02630 J_C[5] = loc3 * matr[8] + loc4 * dg[8]; 02631 02632 loc3 = matr[2] * f + dg[2] * cross; 02633 loc4 = dg[2] * g + matr[2] * cross; 02634 02635 J_A[5] = loc0 + loc3 * matr[2] + loc4 * dg[2]; 02636 J_B[6] = loc3 * matr[3] + loc4 * dg[3]; 02637 J_B[7] = loc3 * matr[4] + loc4 * dg[4]; 02638 J_B[8] = loc3 * matr[5] + loc4 * dg[5]; 02639 J_C[6] = loc3 * matr[6] + loc4 * dg[6]; 02640 J_C[7] = loc3 * matr[7] + loc4 * dg[7]; 02641 J_C[8] = loc3 * matr[8] + loc4 * dg[8]; 02642 02643 /* First diagonal block */ 02644 A[0] = -J_A[0] - J_A[1]; 02645 A[1] = J_A[0] - h * J_A[2]; 02646 A[2] = J_A[1] - h * J_A[2]; 02647 A[3] = th * J_A[2]; 02648 02649 A[4] = -J_A[1] - J_A[3]; 02650 A[5] = J_A[1] - h * J_A[4]; 02651 A[6] = J_A[3] - h * J_A[4]; 02652 A[7] = th * J_A[4]; 02653 02654 A[8] = -J_A[2] - J_A[4]; 02655 A[9] = J_A[2] - h * J_A[5]; 02656 A[10] = J_A[4] - h * J_A[5]; 02657 A[11] = th * J_A[5]; 02658 02659 h_obj[0][0][0] = -A[0] - A[4]; 02660 h_obj[1][0][0] = A[0] - h * A[8]; 02661 h_obj[2][0][0] = A[4] - h * A[8]; 02662 h_obj[3][0][0] = th * A[8]; 02663 02664 h_obj[4][0][0] = A[1] - h * A[9]; 02665 h_obj[5][0][0] = A[5] - h * A[9]; 02666 h_obj[6][0][0] = th * A[9]; 02667 02668 h_obj[7][0][0] = A[6] - h * A[10]; 02669 h_obj[8][0][0] = th * A[10]; 02670 02671 h_obj[9][0][0] = th * A[11]; 02672 02673 /* First off-diagonal block */ 02674 loc2 = matr[8] * loc1; 02675 J_B[1] += loc2; 02676 J_B[3] -= loc2; 02677 02678 loc2 = matr[7] * loc1; 02679 J_B[2] -= loc2; 02680 J_B[6] += loc2; 02681 02682 loc2 = matr[6] * loc1; 02683 J_B[5] += loc2; 02684 J_B[7] -= loc2; 02685 02686 A[0] = -J_B[0] - J_B[1]; 02687 A[1] = J_B[0] - h * J_B[2]; 02688 A[2] = J_B[1] - h * J_B[2]; 02689 A[3] = th * J_B[2]; 02690 02691 A[4] = -J_B[3] - J_B[4]; 02692 A[5] = J_B[3] - h * J_B[5]; 02693 A[6] = J_B[4] - h * J_B[5]; 02694 A[7] = th * J_B[5]; 02695 02696 A[8] = -J_B[6] - J_B[7]; 02697 A[9] = J_B[6] - h * J_B[8]; 02698 A[10] = J_B[7] - h * J_B[8]; 02699 A[11] = th * J_B[8]; 02700 02701 h_obj[0][0][1] = -A[0] - A[4]; 02702 h_obj[1][1][0] = A[0] - h * A[8]; 02703 h_obj[2][1][0] = A[4] - h * A[8]; 02704 h_obj[3][1][0] = th * A[8]; 02705 02706 h_obj[1][0][1] = -A[1] - A[5]; 02707 h_obj[4][0][1] = A[1] - h * A[9]; 02708 h_obj[5][1][0] = A[5] - h * A[9]; 02709 h_obj[6][1][0] = th * A[9]; 02710 02711 h_obj[2][0][1] = -A[2] - A[6]; 02712 h_obj[5][0][1] = A[2] - h * A[10]; 02713 h_obj[7][0][1] = A[6] - h * A[10]; 02714 h_obj[8][1][0] = th * A[10]; 02715 02716 h_obj[3][0][1] = -A[3] - A[7]; 02717 h_obj[6][0][1] = A[3] - h * A[11]; 02718 h_obj[8][0][1] = A[7] - h * A[11]; 02719 h_obj[9][0][1] = th * A[11]; 02720 02721 /* Second off-diagonal block */ 02722 loc2 = matr[5] * loc1; 02723 J_C[1] -= loc2; 02724 J_C[3] += loc2; 02725 02726 loc2 = matr[4] * loc1; 02727 J_C[2] += loc2; 02728 J_C[6] -= loc2; 02729 02730 loc2 = matr[3] * loc1; 02731 J_C[5] -= loc2; 02732 J_C[7] += loc2; 02733 02734 A[0] = -J_C[0] - J_C[1]; 02735 A[1] = J_C[0] - h * J_C[2]; 02736 A[2] = J_C[1] - h * J_C[2]; 02737 A[3] = th * J_C[2]; 02738 02739 A[4] = -J_C[3] - J_C[4]; 02740 A[5] = J_C[3] - h * J_C[5]; 02741 A[6] = J_C[4] - h * J_C[5]; 02742 A[7] = th * J_C[5]; 02743 02744 A[8] = -J_C[6] - J_C[7]; 02745 A[9] = J_C[6] - h * J_C[8]; 02746 A[10] = J_C[7] - h * J_C[8]; 02747 A[11] = th * J_C[8]; 02748 02749 h_obj[0][0][2] = -A[0] - A[4]; 02750 h_obj[1][2][0] = A[0] - h * A[8]; 02751 h_obj[2][2][0] = A[4] - h * A[8]; 02752 h_obj[3][2][0] = th * A[8]; 02753 02754 h_obj[1][0][2] = -A[1] - A[5]; 02755 h_obj[4][0][2] = A[1] - h * A[9]; 02756 h_obj[5][2][0] = A[5] - h * A[9]; 02757 h_obj[6][2][0] = th * A[9]; 02758 02759 h_obj[2][0][2] = -A[2] - A[6]; 02760 h_obj[5][0][2] = A[2] - h * A[10]; 02761 h_obj[7][0][2] = A[6] - h * A[10]; 02762 h_obj[8][2][0] = th * A[10]; 02763 02764 h_obj[3][0][2] = -A[3] - A[7]; 02765 h_obj[6][0][2] = A[3] - h * A[11]; 02766 h_obj[8][0][2] = A[7] - h * A[11]; 02767 h_obj[9][0][2] = th * A[11]; 02768 02769 /* Second block of rows */ 02770 loc3 = matr[3] * f + dg[3] * cross; 02771 loc4 = dg[3] * g + matr[3] * cross; 02772 02773 J_A[0] = loc0 + loc3 * matr[3] + loc4 * dg[3]; 02774 J_A[1] = loc3 * matr[4] + loc4 * dg[4]; 02775 J_A[2] = loc3 * matr[5] + loc4 * dg[5]; 02776 J_B[0] = loc3 * matr[6] + loc4 * dg[6]; 02777 J_B[1] = loc3 * matr[7] + loc4 * dg[7]; 02778 J_B[2] = loc3 * matr[8] + loc4 * dg[8]; 02779 02780 loc3 = matr[4] * f + dg[4] * cross; 02781 loc4 = dg[4] * g + matr[4] * cross; 02782 02783 J_A[3] = loc0 + loc3 * matr[4] + loc4 * dg[4]; 02784 J_A[4] = loc3 * matr[5] + loc4 * dg[5]; 02785 J_B[3] = loc3 * matr[6] + loc4 * dg[6]; 02786 J_B[4] = loc3 * matr[7] + loc4 * dg[7]; 02787 J_B[5] = loc3 * matr[8] + loc4 * dg[8]; 02788 02789 loc3 = matr[5] * f + dg[5] * cross; 02790 loc4 = dg[5] * g + matr[5] * cross; 02791 02792 J_A[5] = loc0 + loc3 * matr[5] + loc4 * dg[5]; 02793 J_B[6] = loc3 * matr[6] + loc4 * dg[6]; 02794 J_B[7] = loc3 * matr[7] + loc4 * dg[7]; 02795 J_B[8] = loc3 * matr[8] + loc4 * dg[8]; 02796 02797 /* Second diagonal block */ 02798 A[0] = -J_A[0] - J_A[1]; 02799 A[1] = J_A[0] - h * J_A[2]; 02800 A[2] = J_A[1] - h * J_A[2]; 02801 A[3] = th * J_A[2]; 02802 02803 A[4] = -J_A[1] - J_A[3]; 02804 A[5] = J_A[1] - h * J_A[4]; 02805 A[6] = J_A[3] - h * J_A[4]; 02806 A[7] = th * J_A[4]; 02807 02808 A[8] = -J_A[2] - J_A[4]; 02809 A[9] = J_A[2] - h * J_A[5]; 02810 A[10] = J_A[4] - h * J_A[5]; 02811 A[11] = th * J_A[5]; 02812 02813 h_obj[0][1][1] = -A[0] - A[4]; 02814 h_obj[1][1][1] = A[0] - h * A[8]; 02815 h_obj[2][1][1] = A[4] - h * A[8]; 02816 h_obj[3][1][1] = th * A[8]; 02817 02818 h_obj[4][1][1] = A[1] - h * A[9]; 02819 h_obj[5][1][1] = A[5] - h * A[9]; 02820 h_obj[6][1][1] = th * A[9]; 02821 02822 h_obj[7][1][1] = A[6] - h * A[10]; 02823 h_obj[8][1][1] = th * A[10]; 02824 02825 h_obj[9][1][1] = th * A[11]; 02826 02827 /* Third off-diagonal block */ 02828 loc2 = matr[2] * loc1; 02829 J_B[1] += loc2; 02830 J_B[3] -= loc2; 02831 02832 loc2 = matr[1] * loc1; 02833 J_B[2] -= loc2; 02834 J_B[6] += loc2; 02835 02836 loc2 = matr[0] * loc1; 02837 J_B[5] += loc2; 02838 J_B[7] -= loc2; 02839 02840 A[0] = -J_B[0] - J_B[1]; 02841 A[1] = J_B[0] - h * J_B[2]; 02842 A[2] = J_B[1] - h * J_B[2]; 02843 A[3] = th * J_B[2]; 02844 02845 A[4] = -J_B[3] - J_B[4]; 02846 A[5] = J_B[3] - h * J_B[5]; 02847 A[6] = J_B[4] - h * J_B[5]; 02848 A[7] = th * J_B[5]; 02849 02850 A[8] = -J_B[6] - J_B[7]; 02851 A[9] = J_B[6] - h * J_B[8]; 02852 A[10] = J_B[7] - h * J_B[8]; 02853 A[11] = th * J_B[8]; 02854 02855 h_obj[0][1][2] = -A[0] - A[4]; 02856 h_obj[1][2][1] = A[0] - h * A[8]; 02857 h_obj[2][2][1] = A[4] - h * A[8]; 02858 h_obj[3][2][1] = th * A[8]; 02859 02860 h_obj[1][1][2] = -A[1] - A[5]; 02861 h_obj[4][1][2] = A[1] - h * A[9]; 02862 h_obj[5][2][1] = A[5] - h * A[9]; 02863 h_obj[6][2][1] = th * A[9]; 02864 02865 h_obj[2][1][2] = -A[2] - A[6]; 02866 h_obj[5][1][2] = A[2] - h * A[10]; 02867 h_obj[7][1][2] = A[6] - h * A[10]; 02868 h_obj[8][2][1] = th * A[10]; 02869 02870 h_obj[3][1][2] = -A[3] - A[7]; 02871 h_obj[6][1][2] = A[3] - h * A[11]; 02872 h_obj[8][1][2] = A[7] - h * A[11]; 02873 h_obj[9][1][2] = th * A[11]; 02874 02875 /* Third block of rows */ 02876 loc3 = matr[6] * f + dg[6] * cross; 02877 loc4 = dg[6] * g + matr[6] * cross; 02878 02879 J_A[0] = loc0 + loc3 * matr[6] + loc4 * dg[6]; 02880 J_A[1] = loc3 * matr[7] + loc4 * dg[7]; 02881 J_A[2] = loc3 * matr[8] + loc4 * dg[8]; 02882 02883 loc3 = matr[7] * f + dg[7] * cross; 02884 loc4 = dg[7] * g + matr[7] * cross; 02885 02886 J_A[3] = loc0 + loc3 * matr[7] + loc4 * dg[7]; 02887 J_A[4] = loc3 * matr[8] + loc4 * dg[8]; 02888 02889 loc3 = matr[8] * f + dg[8] * cross; 02890 loc4 = dg[8] * g + matr[8] * cross; 02891 02892 J_A[5] = loc0 + loc3 * matr[8] + loc4 * dg[8]; 02893 02894 /* Third diagonal block */ 02895 A[0] = -J_A[0] - J_A[1]; 02896 A[1] = J_A[0] - h * J_A[2]; 02897 A[2] = J_A[1] - h * J_A[2]; 02898 A[3] = th * J_A[2]; 02899 02900 A[4] = -J_A[1] - J_A[3]; 02901 A[5] = J_A[1] - h * J_A[4]; 02902 A[6] = J_A[3] - h * J_A[4]; 02903 A[7] = th * J_A[4]; 02904 02905 A[8] = -J_A[2] - J_A[4]; 02906 A[9] = J_A[2] - h * J_A[5]; 02907 A[10] = J_A[4] - h * J_A[5]; 02908 A[11] = th * J_A[5]; 02909 02910 h_obj[0][2][2] = -A[0] - A[4]; 02911 h_obj[1][2][2] = A[0] - h * A[8]; 02912 h_obj[2][2][2] = A[4] - h * A[8]; 02913 h_obj[3][2][2] = th * A[8]; 02914 02915 h_obj[4][2][2] = A[1] - h * A[9]; 02916 h_obj[5][2][2] = A[5] - h * A[9]; 02917 h_obj[6][2][2] = th * A[9]; 02918 02919 h_obj[7][2][2] = A[6] - h * A[10]; 02920 h_obj[8][2][2] = th * A[10]; 02921 02922 h_obj[9][2][2] = th * A[11]; 02923 02924 #if 0 02925 int i, j, k, l; 02926 double nobj; 02927 Vector3D nx[4]; 02928 Vector3D ng_obj[4]; 02929 02930 for (i = 0; i < 4; ++i) { 02931 nx[i] = x[i]; 02932 ng_obj[i] = 0.0; 02933 } 02934 02935 m_fcn_3p(obj, x, a, b, c); 02936 for (i = 0; i < 4; ++i) { 02937 for (j = 0; j < 3; ++j) { 02938 nx[i][j] += 1e-6; 02939 m_fcn_3p(nobj, nx, a, b, c); 02940 nx[i][j] = x[i][j]; 02941 02942 ng_obj[i][j] = (nobj - obj) / 1e-6; 02943 std::cout << i << " " << j << " " << g_obj[i][j] << " " << ng_obj[i][j] << std::endl; 02944 } 02945 } 02946 std::cout << std::endl; 02947 02948 for (i = 0; i < 4; ++i) { 02949 for (j = 0; j < 3; ++j) { 02950 nx[i][j] += 1e-6; 02951 g_fcn_3p(nobj, ng_obj, nx, a, b, c); 02952 nx[i][j] = x[i][j]; 02953 02954 printf("%d %d", i, j); 02955 for (k = 0; k < 4; ++k) { 02956 for (l = 0; l < 3; ++l) { 02957 printf(" % 5.4e", (ng_obj[k][l] - g_obj[k][l]) / 1e-6); 02958 } 02959 } 02960 printf("\n"); 02961 } 02962 } 02963 02964 for (i = 0; i < 10; ++i) { 02965 std::cout << i << std::endl << h_obj[i] << std::endl << std::endl; 02966 } 02967 #endif 02968 02969 // completes diagonal blocks. 02970 h_obj[0].fill_lower_triangle(); 02971 h_obj[4].fill_lower_triangle(); 02972 h_obj[7].fill_lower_triangle(); 02973 h_obj[9].fill_lower_triangle(); 02974 return true; 02975 } 02976 02977 /********************************************************************* 02978 * Reference tetrahedral elements to corners of an ideal wedge element. 02979 * Vertices should be ordered such that the first three vertices form 02980 * the ideally-equaliteral face of the tetrahedron (the end of the 02981 * wedge) and the first and fourth vertices form the tetrahedral edge 02982 * orthogonal to the ideally-equaliteral face (the lateral edge of the 02983 * edge.) 02984 * 1 1/2 0 1 -1/sqrt(3) 0 02985 * W = 0 sqrt(3)/2 0 inv(W) = 0 2/sqrt(3) 0 02986 * 0 0 1 0 0 1 02987 * 02988 *********************************************************************/ 02989 inline bool m_fcn_3w( double& obj, const Vector3D x[4], const double a, const Exponent& b, const Exponent& c ) 02990 { 02991 double matr[9], f, g; 02992 double loc1, loc2, loc3; 02993 02994 /* Calculate M = A*inv(W). */ 02995 matr[0] = x[1][0] - x[0][0]; 02996 matr[1] = isqrt3 * ( 2 * x[2][0] - x[1][0] - x[0][0] ); 02997 matr[2] = x[3][0] - x[0][0]; 02998 02999 matr[3] = x[1][1] - x[0][1]; 03000 matr[4] = isqrt3 * ( 2 * x[2][1] - x[1][1] - x[0][1] ); 03001 matr[5] = x[3][1] - x[0][1]; 03002 03003 matr[6] = x[1][2] - x[0][2]; 03004 matr[7] = isqrt3 * ( 2 * x[2][2] - x[1][2] - x[0][2] ); 03005 matr[8] = x[3][2] - x[0][2]; 03006 03007 /* Calculate det(M). */ 03008 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 03009 loc2 = matr[5] * matr[6] - matr[3] * matr[8]; 03010 loc3 = matr[3] * matr[7] - matr[4] * matr[6]; 03011 g = matr[0] * loc1 + matr[1] * loc2 + matr[2] * loc3; 03012 if( g < MSQ_MIN ) 03013 { 03014 obj = g; 03015 return false; 03016 } 03017 03018 /* Calculate norm(M). */ 03019 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 03020 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 03021 03022 /* Calculate objective function. */ 03023 obj = a * b.raise( f ) * c.raise( g ); 03024 return true; 03025 } 03026 03027 inline bool g_fcn_3w( double& obj, Vector3D g_obj[4], const Vector3D x[4], const double a, const Exponent& b, 03028 const Exponent& c ) 03029 { 03030 double matr[9], f; 03031 double adj_m[9], g; 03032 double loc1, loc2, loc3; 03033 03034 /* Calculate M = A*inv(W). */ 03035 matr[0] = x[1][0] - x[0][0]; 03036 matr[1] = isqrt3 * ( 2 * x[2][0] - x[1][0] - x[0][0] ); 03037 matr[2] = x[3][0] - x[0][0]; 03038 03039 matr[3] = x[1][1] - x[0][1]; 03040 matr[4] = isqrt3 * ( 2 * x[2][1] - x[1][1] - x[0][1] ); 03041 matr[5] = x[3][1] - x[0][1]; 03042 03043 matr[6] = x[1][2] - x[0][2]; 03044 matr[7] = isqrt3 * ( 2 * x[2][2] - x[1][2] - x[0][2] ); 03045 matr[8] = x[3][2] - x[0][2]; 03046 03047 /* Calculate det(M). */ 03048 loc1 = matr[4] * matr[8] - matr[5] * matr[7]; 03049 loc2 = matr[5] * matr[6] - matr[3] * matr[8]; 03050 loc3 = matr[3] * matr[7] - matr[4] * matr[6]; 03051 g = matr[0] * loc1 + matr[1] * loc2 + matr[2] * loc3; 03052 if( g < MSQ_MIN ) 03053 { 03054 obj = g; 03055 return false; 03056 } 03057 03058 /* Calculate norm(M). */ 03059 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 03060 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 03061 03062 /* Calculate objective function. */ 03063 obj = a * b.raise( f ) * c.raise( g ); 03064 03065 /* Calculate the derivative of the objective function. */ 03066 f = b.value() * obj / f; /* Constant on nabla f */ 03067 g = c.value() * obj / g; /* Constant on nable g */ 03068 f *= 2.0; /* Modification for nabla f */ 03069 03070 adj_m[0] = matr[0] * f + loc1 * g; 03071 adj_m[1] = matr[1] * f + loc2 * g; 03072 adj_m[2] = matr[2] * f + loc3 * g; 03073 03074 loc1 = matr[0] * g; 03075 loc2 = matr[1] * g; 03076 loc3 = matr[2] * g; 03077 03078 adj_m[3] = matr[3] * f + loc3 * matr[7] - loc2 * matr[8]; 03079 adj_m[4] = matr[4] * f + loc1 * matr[8] - loc3 * matr[6]; 03080 adj_m[5] = matr[5] * f + loc2 * matr[6] - loc1 * matr[7]; 03081 03082 adj_m[6] = matr[6] * f + loc2 * matr[5] - loc3 * matr[4]; 03083 adj_m[7] = matr[7] * f + loc3 * matr[3] - loc1 * matr[5]; 03084 adj_m[8] = matr[8] * f + loc1 * matr[4] - loc2 * matr[3]; 03085 03086 g_obj[0][0] = -adj_m[0] - isqrt3 * adj_m[1] - adj_m[2]; 03087 g_obj[1][0] = adj_m[0] - isqrt3 * adj_m[1]; 03088 g_obj[2][0] = tisqrt3 * adj_m[1]; 03089 g_obj[3][0] = adj_m[2]; 03090 03091 g_obj[0][1] = -adj_m[3] - isqrt3 * adj_m[4] - adj_m[5]; 03092 g_obj[1][1] = adj_m[3] - isqrt3 * adj_m[4]; 03093 g_obj[2][1] = tisqrt3 * adj_m[4]; 03094 g_obj[3][1] = adj_m[5]; 03095 03096 g_obj[0][2] = -adj_m[6] - isqrt3 * adj_m[7] - adj_m[8]; 03097 g_obj[1][2] = adj_m[6] - isqrt3 * adj_m[7]; 03098 g_obj[2][2] = tisqrt3 * adj_m[7]; 03099 g_obj[3][2] = adj_m[8]; 03100 03101 return true; 03102 } 03103 03104 inline bool h_fcn_3w( double& obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const double a, 03105 const Exponent& b, const Exponent& c ) 03106 { 03107 double matr[9], f; 03108 double adj_m[9], g; 03109 double dg[9], loc0, loc1, loc2, loc3, loc4; 03110 double A[12], J_A[6], J_B[9], J_C[9], cross; 03111 03112 /* Calculate M = A*inv(W). */ 03113 matr[0] = x[1][0] - x[0][0]; 03114 matr[1] = isqrt3 * ( 2 * x[2][0] - x[1][0] - x[0][0] ); 03115 matr[2] = x[3][0] - x[0][0]; 03116 03117 matr[3] = x[1][1] - x[0][1]; 03118 matr[4] = isqrt3 * ( 2 * x[2][1] - x[1][1] - x[0][1] ); 03119 matr[5] = x[3][1] - x[0][1]; 03120 03121 matr[6] = x[1][2] - x[0][2]; 03122 matr[7] = isqrt3 * ( 2 * x[2][2] - x[1][2] - x[0][2] ); 03123 matr[8] = x[3][2] - x[0][2]; 03124 03125 /* Calculate det(M). */ 03126 dg[0] = matr[4] * matr[8] - matr[5] * matr[7]; 03127 dg[1] = matr[5] * matr[6] - matr[3] * matr[8]; 03128 dg[2] = matr[3] * matr[7] - matr[4] * matr[6]; 03129 g = matr[0] * dg[0] + matr[1] * dg[1] + matr[2] * dg[2]; 03130 if( g < MSQ_MIN ) 03131 { 03132 obj = g; 03133 return false; 03134 } 03135 03136 dg[3] = matr[2] * matr[7] - matr[1] * matr[8]; 03137 dg[4] = matr[0] * matr[8] - matr[2] * matr[6]; 03138 dg[5] = matr[1] * matr[6] - matr[0] * matr[7]; 03139 dg[6] = matr[1] * matr[5] - matr[2] * matr[4]; 03140 dg[7] = matr[2] * matr[3] - matr[0] * matr[5]; 03141 dg[8] = matr[0] * matr[4] - matr[1] * matr[3]; 03142 03143 /* Calculate norm(M). */ 03144 f = matr[0] * matr[0] + matr[1] * matr[1] + matr[2] * matr[2] + matr[3] * matr[3] + matr[4] * matr[4] + 03145 matr[5] * matr[5] + matr[6] * matr[6] + matr[7] * matr[7] + matr[8] * matr[8]; 03146 03147 loc3 = f; 03148 loc4 = g; 03149 03150 /* Calculate objective function. */ 03151 obj = a * b.raise( f ) * c.raise( g ); 03152 03153 /* Calculate the derivative of the objective function. */ 03154 f = b.value() * obj / f; /* Constant on nabla f */ 03155 g = c.value() * obj / g; /* Constant on nable g */ 03156 f *= 2.0; /* Modification for nabla f */ 03157 03158 adj_m[0] = matr[0] * f + dg[0] * g; 03159 adj_m[1] = matr[1] * f + dg[1] * g; 03160 adj_m[2] = matr[2] * f + dg[2] * g; 03161 adj_m[3] = matr[3] * f + dg[3] * g; 03162 adj_m[4] = matr[4] * f + dg[4] * g; 03163 adj_m[5] = matr[5] * f + dg[5] * g; 03164 adj_m[6] = matr[6] * f + dg[6] * g; 03165 adj_m[7] = matr[7] * f + dg[7] * g; 03166 adj_m[8] = matr[8] * f + dg[8] * g; 03167 03168 g_obj[0][0] = -adj_m[0] - isqrt3 * adj_m[1] - adj_m[2]; 03169 g_obj[1][0] = adj_m[0] - isqrt3 * adj_m[1]; 03170 g_obj[2][0] = tisqrt3 * adj_m[1]; 03171 g_obj[3][0] = adj_m[2]; 03172 03173 g_obj[0][1] = -adj_m[3] - isqrt3 * adj_m[4] - adj_m[5]; 03174 g_obj[1][1] = adj_m[3] - isqrt3 * adj_m[4]; 03175 g_obj[2][1] = tisqrt3 * adj_m[4]; 03176 g_obj[3][1] = adj_m[5]; 03177 03178 g_obj[0][2] = -adj_m[6] - isqrt3 * adj_m[7] - adj_m[8]; 03179 g_obj[1][2] = adj_m[6] - isqrt3 * adj_m[7]; 03180 g_obj[2][2] = tisqrt3 * adj_m[7]; 03181 g_obj[3][2] = adj_m[8]; 03182 03183 /* Calculate the hessian of the objective. */ 03184 loc0 = f; /* Constant on nabla^2 f */ 03185 loc1 = g; /* Constant on nabla^2 g */ 03186 cross = f * c.value() / loc4; /* Constant on nabla g nabla f */ 03187 f = f * ( b.value() - 1 ) / loc3; /* Constant on nabla f nabla f */ 03188 g = g * ( c.value() - 1 ) / loc4; /* Constant on nabla g nabla g */ 03189 f *= 2.0; /* Modification for nabla f */ 03190 03191 /* First block of rows */ 03192 loc3 = matr[0] * f + dg[0] * cross; 03193 loc4 = dg[0] * g + matr[0] * cross; 03194 03195 J_A[0] = loc0 + loc3 * matr[0] + loc4 * dg[0]; 03196 J_A[1] = loc3 * matr[1] + loc4 * dg[1]; 03197 J_A[2] = loc3 * matr[2] + loc4 * dg[2]; 03198 J_B[0] = loc3 * matr[3] + loc4 * dg[3]; 03199 J_B[1] = loc3 * matr[4] + loc4 * dg[4]; 03200 J_B[2] = loc3 * matr[5] + loc4 * dg[5]; 03201 J_C[0] = loc3 * matr[6] + loc4 * dg[6]; 03202 J_C[1] = loc3 * matr[7] + loc4 * dg[7]; 03203 J_C[2] = loc3 * matr[8] + loc4 * dg[8]; 03204 03205 loc3 = matr[1] * f + dg[1] * cross; 03206 loc4 = dg[1] * g + matr[1] * cross; 03207 03208 J_A[3] = loc0 + loc3 * matr[1] + loc4 * dg[1]; 03209 J_A[4] = loc3 * matr[2] + loc4 * dg[2]; 03210 J_B[3] = loc3 * matr[3] + loc4 * dg[3]; 03211 J_B[4] = loc3 * matr[4] + loc4 * dg[4]; 03212 J_B[5] = loc3 * matr[5] + loc4 * dg[5]; 03213 J_C[3] = loc3 * matr[6] + loc4 * dg[6]; 03214 J_C[4] = loc3 * matr[7] + loc4 * dg[7]; 03215 J_C[5] = loc3 * matr[8] + loc4 * dg[8]; 03216 03217 loc3 = matr[2] * f + dg[2] * cross; 03218 loc4 = dg[2] * g + matr[2] * cross; 03219 03220 J_A[5] = loc0 + loc3 * matr[2] + loc4 * dg[2]; 03221 J_B[6] = loc3 * matr[3] + loc4 * dg[3]; 03222 J_B[7] = loc3 * matr[4] + loc4 * dg[4]; 03223 J_B[8] = loc3 * matr[5] + loc4 * dg[5]; 03224 J_C[6] = loc3 * matr[6] + loc4 * dg[6]; 03225 J_C[7] = loc3 * matr[7] + loc4 * dg[7]; 03226 J_C[8] = loc3 * matr[8] + loc4 * dg[8]; 03227 03228 /* First diagonal block */ 03229 A[0] = -J_A[0] - isqrt3 * J_A[1] - J_A[2]; 03230 A[1] = J_A[0] - isqrt3 * J_A[1]; 03231 // A[2] = tisqrt3*J_A[1]; 03232 // A[3] = J_A[2]; 03233 03234 A[4] = -J_A[1] - isqrt3 * J_A[3] - J_A[4]; 03235 A[5] = J_A[1] - isqrt3 * J_A[3]; 03236 A[6] = tisqrt3 * J_A[3]; 03237 // A[7] = J_A[4]; 03238 03239 A[8] = -J_A[2] - isqrt3 * J_A[4] - J_A[5]; 03240 A[9] = J_A[2] - isqrt3 * J_A[4]; 03241 A[10] = tisqrt3 * J_A[4]; 03242 A[11] = J_A[5]; 03243 03244 h_obj[0][0][0] = -A[0] - isqrt3 * A[4] - A[8]; 03245 h_obj[1][0][0] = A[0] - isqrt3 * A[4]; 03246 h_obj[2][0][0] = tisqrt3 * A[4]; 03247 h_obj[3][0][0] = A[8]; 03248 03249 h_obj[4][0][0] = A[1] - isqrt3 * A[5]; 03250 h_obj[5][0][0] = tisqrt3 * A[5]; 03251 h_obj[6][0][0] = A[9]; 03252 03253 h_obj[7][0][0] = tisqrt3 * A[6]; 03254 h_obj[8][0][0] = A[10]; 03255 03256 h_obj[9][0][0] = A[11]; 03257 03258 /* First off-diagonal block */ 03259 loc2 = matr[8] * loc1; 03260 J_B[1] += loc2; 03261 J_B[3] -= loc2; 03262 03263 loc2 = matr[7] * loc1; 03264 J_B[2] -= loc2; 03265 J_B[6] += loc2; 03266 03267 loc2 = matr[6] * loc1; 03268 J_B[5] += loc2; 03269 J_B[7] -= loc2; 03270 03271 A[0] = -J_B[0] - isqrt3 * J_B[1] - J_B[2]; 03272 A[1] = J_B[0] - isqrt3 * J_B[1]; 03273 A[2] = tisqrt3 * J_B[1]; 03274 A[3] = J_B[2]; 03275 03276 A[4] = -J_B[3] - isqrt3 * J_B[4] - J_B[5]; 03277 A[5] = J_B[3] - isqrt3 * J_B[4]; 03278 A[6] = tisqrt3 * J_B[4]; 03279 A[7] = J_B[5]; 03280 03281 A[8] = -J_B[6] - isqrt3 * J_B[7] - J_B[8]; 03282 A[9] = J_B[6] - isqrt3 * J_B[7]; 03283 A[10] = tisqrt3 * J_B[7]; 03284 A[11] = J_B[8]; 03285 03286 h_obj[0][0][1] = -A[0] - isqrt3 * A[4] - A[8]; 03287 h_obj[1][1][0] = A[0] - isqrt3 * A[4]; 03288 h_obj[2][1][0] = tisqrt3 * A[4]; 03289 h_obj[3][1][0] = A[8]; 03290 03291 h_obj[1][0][1] = -A[1] - isqrt3 * A[5] - A[9]; 03292 h_obj[4][0][1] = A[1] - isqrt3 * A[5]; 03293 h_obj[5][1][0] = tisqrt3 * A[5]; 03294 h_obj[6][1][0] = A[9]; 03295 03296 h_obj[2][0][1] = -A[2] - isqrt3 * A[6] - A[10]; 03297 h_obj[5][0][1] = A[2] - isqrt3 * A[6]; 03298 h_obj[7][0][1] = tisqrt3 * A[6]; 03299 h_obj[8][1][0] = A[10]; 03300 03301 h_obj[3][0][1] = -A[3] - isqrt3 * A[7] - A[11]; 03302 h_obj[6][0][1] = A[3] - isqrt3 * A[7]; 03303 h_obj[8][0][1] = tisqrt3 * A[7]; 03304 h_obj[9][0][1] = A[11]; 03305 03306 /* Second off-diagonal block */ 03307 loc2 = matr[5] * loc1; 03308 J_C[1] -= loc2; 03309 J_C[3] += loc2; 03310 03311 loc2 = matr[4] * loc1; 03312 J_C[2] += loc2; 03313 J_C[6] -= loc2; 03314 03315 loc2 = matr[3] * loc1; 03316 J_C[5] -= loc2; 03317 J_C[7] += loc2; 03318 03319 A[0] = -J_C[0] - isqrt3 * J_C[1] - J_C[2]; 03320 A[1] = J_C[0] - isqrt3 * J_C[1]; 03321 A[2] = tisqrt3 * J_C[1]; 03322 A[3] = J_C[2]; 03323 03324 A[4] = -J_C[3] - isqrt3 * J_C[4] - J_C[5]; 03325 A[5] = J_C[3] - isqrt3 * J_C[4]; 03326 A[6] = tisqrt3 * J_C[4]; 03327 A[7] = J_C[5]; 03328 03329 A[8] = -J_C[6] - isqrt3 * J_C[7] - J_C[8]; 03330 A[9] = J_C[6] - isqrt3 * J_C[7]; 03331 A[10] = tisqrt3 * J_C[7]; 03332 A[11] = J_C[8]; 03333 03334 h_obj[0][0][2] = -A[0] - isqrt3 * A[4] - A[8]; 03335 h_obj[1][2][0] = A[0] - isqrt3 * A[4]; 03336 h_obj[2][2][0] = tisqrt3 * A[4]; 03337 h_obj[3][2][0] = A[8]; 03338 03339 h_obj[1][0][2] = -A[1] - isqrt3 * A[5] - A[9]; 03340 h_obj[4][0][2] = A[1] - isqrt3 * A[5]; 03341 h_obj[5][2][0] = tisqrt3 * A[5]; 03342 h_obj[6][2][0] = A[9]; 03343 03344 h_obj[2][0][2] = -A[2] - isqrt3 * A[6] - A[10]; 03345 h_obj[5][0][2] = A[2] - isqrt3 * A[6]; 03346 h_obj[7][0][2] = tisqrt3 * A[6]; 03347 h_obj[8][2][0] = A[10]; 03348 03349 h_obj[3][0][2] = -A[3] - isqrt3 * A[7] - A[11]; 03350 h_obj[6][0][2] = A[3] - isqrt3 * A[7]; 03351 h_obj[8][0][2] = tisqrt3 * A[7]; 03352 h_obj[9][0][2] = A[11]; 03353 03354 /* Second block of rows */ 03355 loc3 = matr[3] * f + dg[3] * cross; 03356 loc4 = dg[3] * g + matr[3] * cross; 03357 03358 J_A[0] = loc0 + loc3 * matr[3] + loc4 * dg[3]; 03359 J_A[1] = loc3 * matr[4] + loc4 * dg[4]; 03360 J_A[2] = loc3 * matr[5] + loc4 * dg[5]; 03361 J_B[0] = loc3 * matr[6] + loc4 * dg[6]; 03362 J_B[1] = loc3 * matr[7] + loc4 * dg[7]; 03363 J_B[2] = loc3 * matr[8] + loc4 * dg[8]; 03364 03365 loc3 = matr[4] * f + dg[4] * cross; 03366 loc4 = dg[4] * g + matr[4] * cross; 03367 03368 J_A[3] = loc0 + loc3 * matr[4] + loc4 * dg[4]; 03369 J_A[4] = loc3 * matr[5] + loc4 * dg[5]; 03370 J_B[3] = loc3 * matr[6] + loc4 * dg[6]; 03371 J_B[4] = loc3 * matr[7] + loc4 * dg[7]; 03372 J_B[5] = loc3 * matr[8] + loc4 * dg[8]; 03373 03374 loc3 = matr[5] * f + dg[5] * cross; 03375 loc4 = dg[5] * g + matr[5] * cross; 03376 03377 J_A[5] = loc0 + loc3 * matr[5] + loc4 * dg[5]; 03378 J_B[6] = loc3 * matr[6] + loc4 * dg[6]; 03379 J_B[7] = loc3 * matr[7] + loc4 * dg[7]; 03380 J_B[8] = loc3 * matr[8] + loc4 * dg[8]; 03381 03382 /* Second diagonal block */ 03383 A[0] = -J_A[0] - isqrt3 * J_A[1] - J_A[2]; 03384 A[1] = J_A[0] - isqrt3 * J_A[1]; 03385 // A[2] = tisqrt3*J_A[1]; 03386 // A[3] = J_A[2]; 03387 03388 A[4] = -J_A[1] - isqrt3 * J_A[3] - J_A[4]; 03389 A[5] = J_A[1] - isqrt3 * J_A[3]; 03390 A[6] = tisqrt3 * J_A[3]; 03391 // A[7] = J_A[4]; 03392 03393 A[8] = -J_A[2] - isqrt3 * J_A[4] - J_A[5]; 03394 A[9] = J_A[2] - isqrt3 * J_A[4]; 03395 A[10] = tisqrt3 * J_A[4]; 03396 A[11] = J_A[5]; 03397 03398 h_obj[0][1][1] = -A[0] - isqrt3 * A[4] - A[8]; 03399 h_obj[1][1][1] = A[0] - isqrt3 * A[4]; 03400 h_obj[2][1][1] = tisqrt3 * A[4]; 03401 h_obj[3][1][1] = A[8]; 03402 03403 h_obj[4][1][1] = A[1] - isqrt3 * A[5]; 03404 h_obj[5][1][1] = tisqrt3 * A[5]; 03405 h_obj[6][1][1] = A[9]; 03406 03407 h_obj[7][1][1] = tisqrt3 * A[6]; 03408 h_obj[8][1][1] = A[10]; 03409 03410 h_obj[9][1][1] = A[11]; 03411 03412 /* Third off-diagonal block */ 03413 loc2 = matr[2] * loc1; 03414 J_B[1] += loc2; 03415 J_B[3] -= loc2; 03416 03417 loc2 = matr[1] * loc1; 03418 J_B[2] -= loc2; 03419 J_B[6] += loc2; 03420 03421 loc2 = matr[0] * loc1; 03422 J_B[5] += loc2; 03423 J_B[7] -= loc2; 03424 03425 A[0] = -J_B[0] - isqrt3 * J_B[1] - J_B[2]; 03426 A[1] = J_B[0] - isqrt3 * J_B[1]; 03427 A[2] = tisqrt3 * J_B[1]; 03428 A[3] = J_B[2]; 03429 03430 A[4] = -J_B[3] - isqrt3 * J_B[4] - J_B[5]; 03431 A[5] = J_B[3] - isqrt3 * J_B[4]; 03432 A[6] = tisqrt3 * J_B[4]; 03433 A[7] = J_B[5]; 03434 03435 A[8] = -J_B[6] - isqrt3 * J_B[7] - J_B[8]; 03436 A[9] = J_B[6] - isqrt3 * J_B[7]; 03437 A[10] = tisqrt3 * J_B[7]; 03438 A[11] = J_B[8]; 03439 03440 h_obj[0][1][2] = -A[0] - isqrt3 * A[4] - A[8]; 03441 h_obj[1][2][1] = A[0] - isqrt3 * A[4]; 03442 h_obj[2][2][1] = tisqrt3 * A[4]; 03443 h_obj[3][2][1] = A[8]; 03444 03445 h_obj[1][1][2] = -A[1] - isqrt3 * A[5] - A[9]; 03446 h_obj[4][1][2] = A[1] - isqrt3 * A[5]; 03447 h_obj[5][2][1] = tisqrt3 * A[5]; 03448 h_obj[6][2][1] = A[9]; 03449 03450 h_obj[2][1][2] = -A[2] - isqrt3 * A[6] - A[10]; 03451 h_obj[5][1][2] = A[2] - isqrt3 * A[6]; 03452 h_obj[7][1][2] = tisqrt3 * A[6]; 03453 h_obj[8][2][1] = A[10]; 03454 03455 h_obj[3][1][2] = -A[3] - isqrt3 * A[7] - A[11]; 03456 h_obj[6][1][2] = A[3] - isqrt3 * A[7]; 03457 h_obj[8][1][2] = tisqrt3 * A[7]; 03458 h_obj[9][1][2] = A[11]; 03459 03460 /* Third block of rows */ 03461 loc3 = matr[6] * f + dg[6] * cross; 03462 loc4 = dg[6] * g + matr[6] * cross; 03463 03464 J_A[0] = loc0 + loc3 * matr[6] + loc4 * dg[6]; 03465 J_A[1] = loc3 * matr[7] + loc4 * dg[7]; 03466 J_A[2] = loc3 * matr[8] + loc4 * dg[8]; 03467 03468 loc3 = matr[7] * f + dg[7] * cross; 03469 loc4 = dg[7] * g + matr[7] * cross; 03470 03471 J_A[3] = loc0 + loc3 * matr[7] + loc4 * dg[7]; 03472 J_A[4] = loc3 * matr[8] + loc4 * dg[8]; 03473 03474 loc3 = matr[8] * f + dg[8] * cross; 03475 loc4 = dg[8] * g + matr[8] * cross; 03476 03477 J_A[5] = loc0 + loc3 * matr[8] + loc4 * dg[8]; 03478 03479 /* Third diagonal block */ 03480 A[0] = -J_A[0] - isqrt3 * J_A[1] - J_A[2]; 03481 A[1] = J_A[0] - isqrt3 * J_A[1]; 03482 // A[2] = tisqrt3*J_A[1]; 03483 // A[3] = J_A[2]; 03484 03485 A[4] = -J_A[1] - isqrt3 * J_A[3] - J_A[4]; 03486 A[5] = J_A[1] - isqrt3 * J_A[3]; 03487 A[6] = tisqrt3 * J_A[3]; 03488 // A[7] = J_A[4]; 03489 03490 A[8] = -J_A[2] - isqrt3 * J_A[4] - J_A[5]; 03491 A[9] = J_A[2] - isqrt3 * J_A[4]; 03492 A[10] = tisqrt3 * J_A[4]; 03493 A[11] = J_A[5]; 03494 03495 h_obj[0][2][2] = -A[0] - isqrt3 * A[4] - A[8]; 03496 h_obj[1][2][2] = A[0] - isqrt3 * A[4]; 03497 h_obj[2][2][2] = tisqrt3 * A[4]; 03498 h_obj[3][2][2] = A[8]; 03499 03500 h_obj[4][2][2] = A[1] - isqrt3 * A[5]; 03501 h_obj[5][2][2] = tisqrt3 * A[5]; 03502 h_obj[6][2][2] = A[9]; 03503 03504 h_obj[7][2][2] = tisqrt3 * A[6]; 03505 h_obj[8][2][2] = A[10]; 03506 03507 h_obj[9][2][2] = A[11]; 03508 03509 // completes diagonal blocks. 03510 h_obj[0].fill_lower_triangle(); 03511 h_obj[4].fill_lower_triangle(); 03512 h_obj[7].fill_lower_triangle(); 03513 h_obj[9].fill_lower_triangle(); 03514 03515 return true; 03516 } 03517 03518 } // namespace MBMesquite 03519 03520 #endif