MOAB: Mesh Oriented datABase  (version 5.2.1)
MeanRatioFunctions.hpp
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines