Rocstar  1.0
Rocstar multiphysics simulation application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
includeLinks/MeanRatioFunctions.hpp
Go to the documentation of this file.
1 /* *****************************************************************
2  MESQUITE -- The Mesh Quality Improvement Toolkit
3 
4  Copyright 2004 Sandia Corporation and Argonne National
5  Laboratory. Under the terms of Contract DE-AC04-94AL85000
6  with Sandia Corporation, the U.S. Government retains certain
7  rights in this software.
8 
9  This library is free software; you can redistribute it and/or
10  modify it under the terms of the GNU Lesser General Public
11  License as published by the Free Software Foundation; either
12  version 2.1 of the License, or (at your option) any later version.
13 
14  This library is distributed in the hope that it will be useful,
15  but WITHOUT ANY WARRANTY; without even the implied warranty of
16  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17  Lesser General Public License for more details.
18 
19  You should have received a copy of the GNU Lesser General Public License
20  (lgpl.txt) along with this library; if not, write to the Free Software
21  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22 
23  diachin2@llnl.gov, djmelan@sandia.gov, mbrewer@sandia.gov,
24  pknupp@sandia.gov, tleurent@mcs.anl.gov, tmunson@mcs.anl.gov
25 
26  ***************************************************************** */
27 // -*- Mode : c++; tab-width: 3; c-tab-always-indent: t; indent-tabs-mode: nil; c-basic-offset: 3 -*-
28 
38 #ifndef MeanRatioFunctions_hpp
39 #define MeanRatioFunctions_hpp
40 
41 #include <math.h>
42 #include "Mesquite.hpp"
43 #include "Vector3D.hpp"
44 #include "Matrix3D.hpp"
45 #include "Exponent.hpp"
46 
47 namespace Mesquite
48 {
49  /***************************************************************************/
50  /* Gradient calculation courtesy of Paul Hovland. The code was modified */
51  /* to reduce the number of flops and intermediate variables, and improve */
52  /* the locality of reference. */
53  /***************************************************************************/
54  /* The Hessian calculation is computes everyting in (x,y,z) blocks and */
55  /* stores only the upper triangular blocks. The results are put into */
56  /* (c1,c2,c3,c4) order prior to returning the Hessian matrix. */
57  /***************************************************************************/
58  /* The form of the function, gradient, and Hessian follows: */
59  /* o(x) = a * pow(f(A(x)), b) * pow(g(A(x)), c) */
60  /* where A(x) is the incidence matrix generated by: */
61  /* [x1-x0 x2-x0 x3-x0] */
62  /* A(x) = [y1-y0 y2-y0 y3-y0] * inv(W) */
63  /* [z1-z0 z2-z0 z3-z0] */
64  /* and f() is the squared Frobenius norm of A(x), and g() is the */
65  /* determinant of A(x). */
66  /* */
67  /* The gradient is calculated as follows: */
68  /* alpha := a*b*pow(f(A(x)),b-1)*pow(g(A(x)),c) */
69  /* beta := a*c*pow(f(A(x)),b)*pow(g(A(x)),c-1) */
70  /* */
71  /* do/dx = (alpha * (df/dA) + beta * (dg/dA)) (dA/dx) */
72  /* */
73  /* (df/dA)_i = 2*A_i */
74  /* (dg/dA)_i = A_j*A_k - A_l*A_m for some {j,k,l,m} */
75  /* */
76  /* d^2o/dx^2 = (dA/dx)' * ((d alpha/dA) * (df/dA) + */
77  /* (d beta/dA) * (dg/dA) */
78  /* alpha * (d^2f/dA^2) */
79  /* beta * (d^2g/dA^2)) * (dA/dx) */
80  /* */
81  /* Note: since A(x) is a linear function, there are no terms involving */
82  /* d^2A/dx^2 since this matrix is zero. */
83  /* */
84  /* gamma := a*b*c*pow(f(A(x)),b-1)*pow(g(A(x)),c-1) */
85  /* delta := a*c*(c-1)*pow(f(A(x)),b)*pow(g(A(x)),c-2) */
86  /* psi := a*b*(b-1)*pow(f(A(x)),b-2)*pow(g(A(x)),c) */
87  /* */
88  /* d^2o/dx^2 = (dA/dx)' * (gamma*((dg/dA)'*(df/dA) + (df/dA)'*(dg/dA)) + */
89  /* delta* (dg/dA)'*(dg/dA) + */
90  /* psi* (df/dA)'*(df/dA) + */
91  /* alpha*(d^2f/dA^2) + */
92  /* beta*(d^2g/dA^2)) * (dA/dx) */
93  /* */
94  /* Note: (df/dA) and (dg/dA) are row vectors and we only calculate the */
95  /* upper triangular part of the inner matrices. */
96  /* */
97  /* For regular tetrahedral elements, we have the following: */
98  /* */
99  /* [-1 1 0 0 ] */
100  /* M = [-sqrt(3) -sqrt(3) 2*sqrt(3) 0 ] */
101  /* [-sqrt(6) -sqrt(6) -sqrt(6) 3*sqrt(6) ] */
102  /* */
103  /* [M 0 0] */
104  /* dA/dx = [0 M 0] */
105  /* [0 0 M] */
106  /***************************************************************************/
107 
108 /*****************************************************************************/
109 /* Not all compilers substitute out constants (especially the square root). */
110 /* Therefore, they are substituted out manually. The values below were */
111 /* calculated on a solaris machine using long doubles. I believe they are */
112 /* accurate. */
113 /*****************************************************************************/
114 
115 #define isqrt3 5.77350269189625797959429519858e-01 /* 1.0/sqrt(3.0)*/
116 #define tisqrt3 1.15470053837925159591885903972e+00 /* 2.0/sqrt(3.0)*/
117 #define isqrt6 4.08248290463863052509822647505e-01 /* 1.0/sqrt(6.0)*/
118 #define tisqrt6 1.22474487139158915752946794252e+00 /* 3.0/sqrt(6.0)*/
119 
120 /*****************************************************************************/
121 /* The following set of functions reference triangular elements to an */
122 /* equilateral triangle in the plane defined by the normal. They are */
123 /* used when assessing the quality of a triangular element. A zero */
124 /* return value indicates success, while a nonzero value indicates failure. */
125 /*****************************************************************************/
126 
127 /*****************************************************************************/
128 /* Function evaluation requires 44 flops. */
129 /* Reductions possible when b == 1 or c == 1 */
130 /*****************************************************************************/
131 inline bool m_fcn_2e(double &obj, const Vector3D x[3], const Vector3D &n,
132  const double a, const Exponent& b, const Exponent& c)
133 {
134  double matr[9], f;
135  double g;
136 
137  /* Calculate M = [A*inv(W) n] */
138  matr[0] = x[1][0] - x[0][0];
139  matr[1] = (2.0*x[2][0] - x[1][0] - x[0][0])*isqrt3;
140  matr[2] = n[0];
141 
142  matr[3] = x[1][1] - x[0][1];
143  matr[4] = (2.0*x[2][1] - x[1][1] - x[0][1])*isqrt3;
144  matr[5] = n[1];
145 
146  matr[6] = x[1][2] - x[0][2];
147  matr[7] = (2.0*x[2][2] - x[1][2] - x[0][2])*isqrt3;
148  matr[8] = n[2];
149 
150  /* Calculate det(M). */
151  g = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
152  matr[3]*(matr[2]*matr[7] - matr[1]*matr[8]) +
153  matr[6]*(matr[1]*matr[5] - matr[2]*matr[4]);
154  if (g < MSQ_MIN) { obj = g; return false; }
155 
156  /* Calculate norm(M). */
157  f = matr[0]*matr[0] + matr[1]*matr[1] +
158  matr[3]*matr[3] + matr[4]*matr[4] +
159  matr[6]*matr[6] + matr[7]*matr[7];
160 
161  /* Calculate objective function. */
162  obj = a * pow(f, b) * pow(g, c);
163  return true;
164 }
165 
166 /*****************************************************************************/
167 /* Gradient evaluation requires 88 flops. */
168 /* Reductions possible when b == 1 or c == 1 */
169 /*****************************************************************************/
170 inline bool g_fcn_2e(double &obj, Vector3D g_obj[3],
171  const Vector3D x[3], const Vector3D &n,
172  const double a, const Exponent& b, const Exponent& c)
173 {
174  double matr[9], f;
175  double adj_m[9], g; // adj_m[2,5,8] not used
176  double loc1, loc2, loc3;
177 
178  /* Calculate M = [A*inv(W) n] */
179  matr[0] = x[1][0] - x[0][0];
180  matr[1] = (2.0*x[2][0] - x[1][0] - x[0][0])*isqrt3;
181  matr[2] = n[0];
182 
183  matr[3] = x[1][1] - x[0][1];
184  matr[4] = (2.0*x[2][1] - x[1][1] - x[0][1])*isqrt3;
185  matr[5] = n[1];
186 
187  matr[6] = x[1][2] - x[0][2];
188  matr[7] = (2.0*x[2][2] - x[1][2] - x[0][2])*isqrt3;
189  matr[8] = n[2];
190 
191  /* Calculate det([n M]). */
192  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
193  loc2 = matr[2]*matr[7] - matr[1]*matr[8];
194  loc3 = matr[1]*matr[5] - matr[2]*matr[4];
195  g = matr[0]*loc1 + matr[3]*loc2 + matr[6]*loc3;
196  if (g < MSQ_MIN) { obj = g; return false; }
197 
198  /* Calculate norm(M). */
199  f = matr[0]*matr[0] + matr[1]*matr[1] +
200  matr[3]*matr[3] + matr[4]*matr[4] +
201  matr[6]*matr[6] + matr[7]*matr[7];
202 
203  /* Calculate objective function. */
204  obj = a * pow(f, b) * pow(g, c);
205 
206  /* Calculate the derivative of the objective function. */
207  f = b * obj / f; /* Constant on nabla f */
208  g = c * obj / g; /* Constant on nable g */
209  f *= 2.0; /* Modification for nabla f */
210 
211  adj_m[0] = matr[0]*f + loc1*g;
212  adj_m[3] = matr[3]*f + loc2*g;
213  adj_m[6] = matr[6]*f + loc3*g;
214 
215  loc1 = matr[0]*g;
216  loc2 = matr[3]*g;
217  loc3 = matr[6]*g;
218 
219  adj_m[1] = matr[1]*f + loc3*matr[5] - loc2*matr[8];
220  adj_m[4] = matr[4]*f + loc1*matr[8] - loc3*matr[2];
221  adj_m[7] = matr[7]*f + loc2*matr[2] - loc1*matr[5];
222 
223  loc1 = isqrt3*adj_m[1];
224  g_obj[0][0] = -adj_m[0] - loc1;
225  g_obj[1][0] = adj_m[0] - loc1;
226  g_obj[2][0] = 2.0*loc1;
227 
228  loc1 = isqrt3*adj_m[4];
229  g_obj[0][1] = -adj_m[3] - loc1;
230  g_obj[1][1] = adj_m[3] - loc1;
231  g_obj[2][1] = 2.0*loc1;
232 
233  loc1 = isqrt3*adj_m[7];
234  g_obj[0][2] = -adj_m[6] - loc1;
235  g_obj[1][2] = adj_m[6] - loc1;
236  g_obj[2][2] = 2.0*loc1;
237  return true;
238 }
239 
240 /*****************************************************************************/
241 /* Hessian evaluation requires 316 flops. */
242 /* Reductions possible when b == 1 or c == 1 */
243 /*****************************************************************************/
244 inline bool h_fcn_2e(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6],
245  const Vector3D x[3], const Vector3D &n,
246  const double a, const Exponent& b, const Exponent& c)
247 {
248  double matr[9], f;
249  double adj_m[9], g; // adj_m[2,5,8] not used
250  double dg[9]; // dg[2,5,8] not used
251  double loc0, loc1, loc2, loc3, loc4;
252  double A[12], J_A[6], J_B[9], J_C[9], cross; // only 2x2 corners used
253 
254  /* Calculate M = [A*inv(W) n] */
255  matr[0] = x[1][0] - x[0][0];
256  matr[1] = (2.0*x[2][0] - x[1][0] - x[0][0])*isqrt3;
257  matr[2] = n[0];
258 
259  matr[3] = x[1][1] - x[0][1];
260  matr[4] = (2.0*x[2][1] - x[1][1] - x[0][1])*isqrt3;
261  matr[5] = n[1];
262 
263  matr[6] = x[1][2] - x[0][2];
264  matr[7] = (2.0*x[2][2] - x[1][2] - x[0][2])*isqrt3;
265  matr[8] = n[2];
266 
267  /* Calculate det([n M]). */
268  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
269  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
270  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
271  g = matr[0]*dg[0] + matr[3]*dg[3] + matr[6]*dg[6];
272  if (g < MSQ_MIN) { obj = g; return false; }
273 
274  /* Calculate norm(M). */
275  f = matr[0]*matr[0] + matr[1]*matr[1] +
276  matr[3]*matr[3] + matr[4]*matr[4] +
277  matr[6]*matr[6] + matr[7]*matr[7];
278 
279  loc3 = f;
280  loc4 = g;
281 
282  /* Calculate objective function. */
283  obj = a * pow(f, b) * pow(g, c);
284 
285  /* Calculate the derivative of the objective function. */
286  f = b * obj / f; /* Constant on nabla f */
287  g = c * obj / g; /* Constant on nable g */
288  f *= 2.0; /* Modification for nabla f */
289 
290  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
291  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
292  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
293 
294  adj_m[0] = matr[0]*f + dg[0]*g;
295  adj_m[1] = matr[1]*f + dg[1]*g;
296  adj_m[3] = matr[3]*f + dg[3]*g;
297  adj_m[4] = matr[4]*f + dg[4]*g;
298  adj_m[6] = matr[6]*f + dg[6]*g;
299  adj_m[7] = matr[7]*f + dg[7]*g;
300 
301  loc1 = isqrt3*adj_m[1];
302  g_obj[0][0] = -adj_m[0] - loc1;
303  g_obj[1][0] = adj_m[0] - loc1;
304  g_obj[2][0] = 2.0*loc1;
305 
306  loc1 = isqrt3*adj_m[4];
307  g_obj[0][1] = -adj_m[3] - loc1;
308  g_obj[1][1] = adj_m[3] - loc1;
309  g_obj[2][1] = 2.0*loc1;
310 
311  loc1 = isqrt3*adj_m[7];
312  g_obj[0][2] = -adj_m[6] - loc1;
313  g_obj[1][2] = adj_m[6] - loc1;
314  g_obj[2][2] = 2.0*loc1;
315 
316  /* Calculate the hessian of the objective. */
317  loc0 = f; /* Constant on nabla^2 f */
318  loc1 = g; /* Constant on nabla^2 g */
319  cross = f * c / loc4; /* Constant on nabla g nabla f */
320  f = f * (b-1) / loc3; /* Constant on nabla f nabla f */
321  g = g * (c-1) / loc4; /* Constant on nabla g nabla g */
322  f *= 2.0; /* Modification for nabla f */
323 
324  /* First block of rows */
325  loc3 = matr[0]*f + dg[0]*cross;
326  loc4 = dg[0]*g + matr[0]*cross;
327 
328  J_A[0] = loc0 + loc3*matr[0] + loc4*dg[0];
329  J_A[1] = loc3*matr[1] + loc4*dg[1];
330  J_B[0] = loc3*matr[3] + loc4*dg[3];
331  J_B[1] = loc3*matr[4] + loc4*dg[4];
332  J_C[0] = loc3*matr[6] + loc4*dg[6];
333  J_C[1] = loc3*matr[7] + loc4*dg[7];
334 
335  loc3 = matr[1]*f + dg[1]*cross;
336  loc4 = dg[1]*g + matr[1]*cross;
337 
338  J_A[3] = loc0 + loc3*matr[1] + loc4*dg[1];
339  J_B[3] = loc3*matr[3] + loc4*dg[3];
340  J_B[4] = loc3*matr[4] + loc4*dg[4];
341  J_C[3] = loc3*matr[6] + loc4*dg[6];
342  J_C[4] = loc3*matr[7] + loc4*dg[7];
343 
344  /* First diagonal block */
345  loc2 = isqrt3*J_A[1];
346  A[0] = -J_A[0] - loc2;
347  A[1] = J_A[0] - loc2;
348 
349  loc2 = isqrt3*J_A[3];
350  A[4] = -J_A[1] - loc2;
351  A[5] = J_A[1] - loc2;
352  A[6] = 2.0*loc2;
353 
354  loc2 = isqrt3*A[4];
355  h_obj[0][0][0] = -A[0] - loc2;
356  h_obj[1][0][0] = A[0] - loc2;
357  h_obj[2][0][0] = 2.0*loc2;
358 
359  loc2 = isqrt3*A[5];
360  h_obj[3][0][0] = A[1] - loc2;
361  h_obj[4][0][0] = 2.0*loc2;
362 
363  h_obj[5][0][0] = tisqrt3*A[6];
364 
365  /* First off-diagonal block */
366  loc2 = matr[8]*loc1;
367  J_B[1] += loc2;
368  J_B[3] -= loc2;
369 
370  loc2 = isqrt3*J_B[3];
371  A[0] = -J_B[0] - loc2;
372  A[1] = J_B[0] - loc2;
373  A[2] = 2.0*loc2;
374 
375  loc2 = isqrt3*J_B[4];
376  A[4] = -J_B[1] - loc2;
377  A[5] = J_B[1] - loc2;
378  A[6] = 2.0*loc2;
379 
380  loc2 = isqrt3*A[4];
381  h_obj[0][0][1] = -A[0] - loc2;
382  h_obj[1][0][1] = A[0] - loc2;
383  h_obj[2][0][1] = 2.0*loc2;
384 
385  loc2 = isqrt3*A[5];
386  h_obj[1][1][0] = -A[1] - loc2;
387  h_obj[3][0][1] = A[1] - loc2;
388  h_obj[4][0][1] = 2.0*loc2;
389 
390  loc2 = isqrt3*A[6];
391  h_obj[2][1][0] = -A[2] - loc2;
392  h_obj[4][1][0] = A[2] - loc2;
393  h_obj[5][0][1] = 2.0*loc2;
394 
395  /* Second off-diagonal block */
396  loc2 = matr[5]*loc1;
397  J_C[1] -= loc2;
398  J_C[3] += loc2;
399 
400  loc2 = isqrt3*J_C[3];
401  A[0] = -J_C[0] - loc2;
402  A[1] = J_C[0] - loc2;
403  A[2] = 2.0*loc2;
404 
405  loc2 = isqrt3*J_C[4];
406  A[4] = -J_C[1] - loc2;
407  A[5] = J_C[1] - loc2;
408  A[6] = 2.0*loc2;
409 
410  loc2 = isqrt3*A[4];
411  h_obj[0][0][2] = -A[0] - loc2;
412  h_obj[1][0][2] = A[0] - loc2;
413  h_obj[2][0][2] = 2.0*loc2;
414 
415  loc2 = isqrt3*A[5];
416  h_obj[1][2][0] = -A[1] - loc2;
417  h_obj[3][0][2] = A[1] - loc2;
418  h_obj[4][0][2] = 2.0*loc2;
419 
420  loc2 = isqrt3*A[6];
421  h_obj[2][2][0] = -A[2] - loc2;
422  h_obj[4][2][0] = A[2] - loc2;
423  h_obj[5][0][2] = 2.0*loc2;
424 
425  /* Second block of rows */
426  loc3 = matr[3]*f + dg[3]*cross;
427  loc4 = dg[3]*g + matr[3]*cross;
428 
429  J_A[0] = loc0 + loc3*matr[3] + loc4*dg[3];
430  J_A[1] = loc3*matr[4] + loc4*dg[4];
431  J_B[0] = loc3*matr[6] + loc4*dg[6];
432  J_B[1] = loc3*matr[7] + loc4*dg[7];
433 
434  loc3 = matr[4]*f + dg[4]*cross;
435  loc4 = dg[4]*g + matr[4]*cross;
436 
437  J_A[3] = loc0 + loc3*matr[4] + loc4*dg[4];
438  J_B[3] = loc3*matr[6] + loc4*dg[6];
439  J_B[4] = loc3*matr[7] + loc4*dg[7];
440 
441  /* Second diagonal block */
442  loc2 = isqrt3*J_A[1];
443  A[0] = -J_A[0] - loc2;
444  A[1] = J_A[0] - loc2;
445 
446  loc2 = isqrt3*J_A[3];
447  A[4] = -J_A[1] - loc2;
448  A[5] = J_A[1] - loc2;
449  A[6] = 2.0*loc2;
450 
451  loc2 = isqrt3*A[4];
452  h_obj[0][1][1] = -A[0] - loc2;
453  h_obj[1][1][1] = A[0] - loc2;
454  h_obj[2][1][1] = 2.0*loc2;
455 
456  loc2 = isqrt3*A[5];
457  h_obj[3][1][1] = A[1] - loc2;
458  h_obj[4][1][1] = 2.0*loc2;
459 
460  h_obj[5][1][1] = tisqrt3*A[6];
461 
462  /* Third off-diagonal block */
463  loc2 = matr[2]*loc1;
464  J_B[1] += loc2;
465  J_B[3] -= loc2;
466 
467  loc2 = isqrt3*J_B[3];
468  A[0] = -J_B[0] - loc2;
469  A[1] = J_B[0] - loc2;
470  A[2] = 2.0*loc2;
471 
472  loc2 = isqrt3*J_B[4];
473  A[4] = -J_B[1] - loc2;
474  A[5] = J_B[1] - loc2;
475  A[6] = 2.0*loc2;
476 
477  loc2 = isqrt3*A[4];
478  h_obj[0][1][2] = -A[0] - loc2;
479  h_obj[1][1][2] = A[0] - loc2;
480  h_obj[2][1][2] = 2.0*loc2;
481 
482  loc2 = isqrt3*A[5];
483  h_obj[1][2][1] = -A[1] - loc2;
484  h_obj[3][1][2] = A[1] - loc2;
485  h_obj[4][1][2] = 2.0*loc2;
486 
487  loc2 = isqrt3*A[6];
488  h_obj[2][2][1] = -A[2] - loc2;
489  h_obj[4][2][1] = A[2] - loc2;
490  h_obj[5][1][2] = 2.0*loc2;
491 
492  /* Third block of rows */
493  loc3 = matr[6]*f + dg[6]*cross;
494  loc4 = dg[6]*g + matr[6]*cross;
495 
496  J_A[0] = loc0 + loc3*matr[6] + loc4*dg[6];
497  J_A[1] = loc3*matr[7] + loc4*dg[7];
498 
499  loc3 = matr[7]*f + dg[7]*cross;
500  loc4 = dg[7]*g + matr[7]*cross;
501 
502  J_A[3] = loc0 + loc3*matr[7] + loc4*dg[7];
503 
504  /* Third diagonal block */
505  loc2 = isqrt3*J_A[1];
506  A[0] = -J_A[0] - loc2;
507  A[1] = J_A[0] - loc2;
508 
509  loc2 = isqrt3*J_A[3];
510  A[4] = -J_A[1] - loc2;
511  A[5] = J_A[1] - loc2;
512  A[6] = 2.0*loc2;
513 
514  loc2 = isqrt3*A[4];
515  h_obj[0][2][2] = -A[0] - loc2;
516  h_obj[1][2][2] = A[0] - loc2;
517  h_obj[2][2][2] = 2.0*loc2;
518 
519  loc2 = isqrt3*A[5];
520  h_obj[3][2][2] = A[1] - loc2;
521  h_obj[4][2][2] = 2.0*loc2;
522 
523  h_obj[5][2][2] = tisqrt3*A[6];
524 
525  // completes diagonal blocks.
526  h_obj[0].fill_lower_triangle();
527  h_obj[3].fill_lower_triangle();
528  h_obj[5].fill_lower_triangle();
529  return true;
530 }
531 
532 /*****************************************************************************/
533 /* The following set of functions reference triangular elements to an */
534 /* right triangle in the plane defined by the normal. They are used when */
535 /* assessing the quality of a quadrilateral elements. A zero return value */
536 /* indicates success, while a nonzero value indicates failure. */
537 /*****************************************************************************/
538 
539 /*****************************************************************************/
540 /* Function evaluation -- requires 41 flops. */
541 /* Reductions possible when b == 1, c == 1, or d == 1 */
542 /*****************************************************************************/
543 inline bool m_fcn_2i(double &obj, const Vector3D x[3], const Vector3D &n,
544  const double a, const Exponent& b, const Exponent& c,
545  const Vector3D &d)
546 {
547  double matr[9];
548  double f;
549  double g;
550 
551  /* Calculate M = A*inv(W). */
552  matr[0] = d[0]*(x[1][0] - x[0][0]);
553  matr[1] = d[1]*(x[2][0] - x[0][0]);
554  matr[2] = n[0];
555 
556  matr[3] = d[0]*(x[1][1] - x[0][1]);
557  matr[4] = d[1]*(x[2][1] - x[0][1]);
558  matr[5] = n[1];
559 
560  matr[6] = d[0]*(x[1][2] - x[0][2]);
561  matr[7] = d[1]*(x[2][2] - x[0][2]);
562  matr[8] = n[2];
563 
564  /* Calculate det(M). */
565  g = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
566  matr[3]*(matr[2]*matr[7] - matr[1]*matr[8]) +
567  matr[6]*(matr[1]*matr[5] - matr[2]*matr[4]);
568  if (g < MSQ_MIN) { obj = g; return false; }
569 
570  /* Calculate norm(M). */
571  f = matr[0]*matr[0] + matr[1]*matr[1] +
572  matr[3]*matr[3] + matr[4]*matr[4] +
573  matr[6]*matr[6] + matr[7]*matr[7];
574 
575  /* Calculate objective function. */
576  obj = a * pow(f, b) * pow(g, c);
577  return true;
578 }
579 
580 /*****************************************************************************/
581 /* Gradient requires 82 flops. */
582 /* Reductions possible when b == 1, c == 1, or d == 1 */
583 /*****************************************************************************/
584 inline bool g_fcn_2i(double &obj, Vector3D g_obj[3],
585  const Vector3D x[3], const Vector3D &n,
586  const double a, const Exponent& b, const Exponent& c,
587  const Vector3D &d)
588 {
589  double matr[9], f;
590  double adj_m[9], g; // adj_m[2,5,8] not used
591  double loc1, loc2, loc3;
592 
593  /* Calculate M = [A*inv(W) n] */
594  matr[0] = d[0]*(x[1][0] - x[0][0]);
595  matr[1] = d[1]*(x[2][0] - x[0][0]);
596  matr[2] = n[0];
597 
598  matr[3] = d[0]*(x[1][1] - x[0][1]);
599  matr[4] = d[1]*(x[2][1] - x[0][1]);
600  matr[5] = n[1];
601 
602  matr[6] = d[0]*(x[1][2] - x[0][2]);
603  matr[7] = d[1]*(x[2][2] - x[0][2]);
604  matr[8] = n[2];
605 
606  /* Calculate det([n M]). */
607  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
608  loc2 = matr[2]*matr[7] - matr[1]*matr[8];
609  loc3 = matr[1]*matr[5] - matr[2]*matr[4];
610  g = matr[0]*loc1 + matr[3]*loc2 + matr[6]*loc3;
611  if (g < MSQ_MIN) { obj = g; return false; }
612 
613  /* Calculate norm(M). */
614  f = matr[0]*matr[0] + matr[1]*matr[1] +
615  matr[3]*matr[3] + matr[4]*matr[4] +
616  matr[6]*matr[6] + matr[7]*matr[7];
617 
618  /* Calculate objective function. */
619  obj = a * pow(f, b) * pow(g, c);
620 
621  /* Calculate the derivative of the objective function. */
622  f = b * obj / f; /* Constant on nabla f */
623  g = c * obj / g; /* Constant on nable g */
624  f *= 2.0; /* Modification for nabla f */
625 
626  adj_m[0] = d[0]*(matr[0]*f + loc1*g);
627  adj_m[3] = d[0]*(matr[3]*f + loc2*g);
628  adj_m[6] = d[0]*(matr[6]*f + loc3*g);
629 
630  loc1 = matr[0]*g;
631  loc2 = matr[3]*g;
632  loc3 = matr[6]*g;
633 
634  adj_m[1] = d[1]*(matr[1]*f + loc3*matr[5] - loc2*matr[8]);
635  adj_m[4] = d[1]*(matr[4]*f + loc1*matr[8] - loc3*matr[2]);
636  adj_m[7] = d[1]*(matr[7]*f + loc2*matr[2] - loc1*matr[5]);
637 
638  g_obj[0][0] = -adj_m[0] - adj_m[1];
639  g_obj[1][0] = adj_m[0];
640  g_obj[2][0] = adj_m[1];
641 
642  g_obj[0][1] = -adj_m[3] - adj_m[4];
643  g_obj[1][1] = adj_m[3];
644  g_obj[2][1] = adj_m[4];
645 
646  g_obj[0][2] = -adj_m[6] - adj_m[7];
647  g_obj[1][2] = adj_m[6];
648  g_obj[2][2] = adj_m[7];
649  return true;
650 }
651 
652 /*****************************************************************************/
653 /* Hessian requires 253 flops. */
654 /* Reductions possible when b == 1, c == 1, or d == 1 */
655 /*****************************************************************************/
656 inline bool h_fcn_2i(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6],
657  const Vector3D x[3], const Vector3D &n,
658  const double a, const Exponent& b, const Exponent& c,
659  const Vector3D &d)
660 {
661  double matr[9], f;
662  double adj_m[9], g; // adj_m[2,5,8] not used
663  double dg[9]; // dg[2,5,8] not used
664  double loc0, loc1, loc2, loc3, loc4;
665  double A[12], J_A[6], J_B[9], J_C[9], cross; // only 2x2 corners used
666 
667  const double scale[3] = {
668  d[0]*d[0], d[0]*d[1],
669  d[1]*d[1]
670  };
671 
672  /* Calculate M = [A*inv(W) n] */
673  matr[0] = d[0]*(x[1][0] - x[0][0]);
674  matr[1] = d[1]*(x[2][0] - x[0][0]);
675  matr[2] = n[0];
676 
677  matr[3] = d[0]*(x[1][1] - x[0][1]);
678  matr[4] = d[1]*(x[2][1] - x[0][1]);
679  matr[5] = n[1];
680 
681  matr[6] = d[0]*(x[1][2] - x[0][2]);
682  matr[7] = d[1]*(x[2][2] - x[0][2]);
683  matr[8] = n[2];
684 
685  /* Calculate det([n M]). */
686  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
687  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
688  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
689  g = matr[0]*dg[0] + matr[3]*dg[3] + matr[6]*dg[6];
690  if (g < MSQ_MIN) { obj = g; return false; }
691 
692  /* Calculate norm(M). */
693  f = matr[0]*matr[0] + matr[1]*matr[1] +
694  matr[3]*matr[3] + matr[4]*matr[4] +
695  matr[6]*matr[6] + matr[7]*matr[7];
696 
697  loc3 = f;
698  loc4 = g;
699 
700  /* Calculate objective function. */
701  obj = a * pow(f, b) * pow(g, c);
702 
703  /* Calculate the derivative of the objective function. */
704  f = b * obj / f; /* Constant on nabla f */
705  g = c * obj / g; /* Constant on nable g */
706  f *= 2.0; /* Modification for nabla f */
707 
708  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
709  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
710  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
711 
712  adj_m[0] = d[0]*(matr[0]*f + dg[0]*g);
713  adj_m[1] = d[1]*(matr[1]*f + dg[1]*g);
714  adj_m[3] = d[0]*(matr[3]*f + dg[3]*g);
715  adj_m[4] = d[1]*(matr[4]*f + dg[4]*g);
716  adj_m[6] = d[0]*(matr[6]*f + dg[6]*g);
717  adj_m[7] = d[1]*(matr[7]*f + dg[7]*g);
718 
719  g_obj[0][0] = -adj_m[0] - adj_m[1];
720  g_obj[1][0] = adj_m[0];
721  g_obj[2][0] = adj_m[1];
722 
723  g_obj[0][1] = -adj_m[3] - adj_m[4];
724  g_obj[1][1] = adj_m[3];
725  g_obj[2][1] = adj_m[4];
726 
727  g_obj[0][2] = -adj_m[6] - adj_m[7];
728  g_obj[1][2] = adj_m[6];
729  g_obj[2][2] = adj_m[7];
730 
731  /* Calculate the hessian of the objective. */
732  loc0 = f; /* Constant on nabla^2 f */
733  loc1 = g; /* Constant on nabla^2 g */
734  cross = f * c / loc4; /* Constant on nabla g nabla f */
735  f = f * (b-1) / loc3; /* Constant on nabla f nabla f */
736  g = g * (c-1) / loc4; /* Constant on nabla g nabla g */
737  f *= 2.0; /* Modification for nabla f */
738 
739  /* First block of rows */
740  loc3 = matr[0]*f + dg[0]*cross;
741  loc4 = dg[0]*g + matr[0]*cross;
742 
743  J_A[0] = loc0 + loc3*matr[0] + loc4*dg[0];
744  J_A[1] = loc3*matr[1] + loc4*dg[1];
745  J_B[0] = loc3*matr[3] + loc4*dg[3];
746  J_B[1] = loc3*matr[4] + loc4*dg[4];
747  J_C[0] = loc3*matr[6] + loc4*dg[6];
748  J_C[1] = loc3*matr[7] + loc4*dg[7];
749 
750  loc3 = matr[1]*f + dg[1]*cross;
751  loc4 = dg[1]*g + matr[1]*cross;
752 
753  J_A[3] = loc0 + loc3*matr[1] + loc4*dg[1];
754  J_B[3] = loc3*matr[3] + loc4*dg[3];
755  J_B[4] = loc3*matr[4] + loc4*dg[4];
756  J_C[3] = loc3*matr[6] + loc4*dg[6];
757  J_C[4] = loc3*matr[7] + loc4*dg[7];
758 
759  /* First diagonal block */
760  J_A[0] *= scale[0];
761  J_A[1] *= scale[1];
762  J_A[3] *= scale[2];
763 
764  A[0] = -J_A[0] - J_A[1];
765  A[4] = -J_A[1] - J_A[3];
766 
767  h_obj[0][0][0] = -A[0] - A[4];
768  h_obj[1][0][0] = A[0];
769  h_obj[2][0][0] = A[4];
770 
771  h_obj[3][0][0] = J_A[0];
772  h_obj[4][0][0] = J_A[1];
773 
774  h_obj[5][0][0] = J_A[3];
775 
776  /* First off-diagonal block */
777  loc2 = matr[8]*loc1;
778  J_B[1] += loc2;
779  J_B[3] -= loc2;
780 
781  J_B[0] *= scale[0];
782  J_B[1] *= scale[1];
783  J_B[3] *= scale[1];
784  J_B[4] *= scale[2];
785 
786  A[0] = -J_B[0] - J_B[3];
787  A[4] = -J_B[1] - J_B[4];
788 
789  h_obj[0][0][1] = -A[0] - A[4];
790  h_obj[1][0][1] = A[0];
791  h_obj[2][0][1] = A[4];
792 
793  h_obj[1][1][0] = -J_B[0] - J_B[1];
794  h_obj[3][0][1] = J_B[0];
795  h_obj[4][0][1] = J_B[1];
796 
797  h_obj[2][1][0] = -J_B[3] - J_B[4];
798  h_obj[4][1][0] = J_B[3];
799  h_obj[5][0][1] = J_B[4];
800 
801  /* Second off-diagonal block */
802  loc2 = matr[5]*loc1;
803  J_C[1] -= loc2;
804  J_C[3] += loc2;
805 
806  J_C[0] *= scale[0];
807  J_C[1] *= scale[1];
808  J_C[3] *= scale[1];
809  J_C[4] *= scale[2];
810 
811  A[0] = -J_C[0] - J_C[3];
812  A[4] = -J_C[1] - J_C[4];
813 
814  h_obj[0][0][2] = -A[0] - A[4];
815  h_obj[1][0][2] = A[0];
816  h_obj[2][0][2] = A[4];
817 
818  h_obj[1][2][0] = -J_C[0] - J_C[1];
819  h_obj[3][0][2] = J_C[0];
820  h_obj[4][0][2] = J_C[1];
821 
822  h_obj[2][2][0] = -J_C[3] - J_C[4];
823  h_obj[4][2][0] = J_C[3];
824  h_obj[5][0][2] = J_C[4];
825 
826  /* Second block of rows */
827  loc3 = matr[3]*f + dg[3]*cross;
828  loc4 = dg[3]*g + matr[3]*cross;
829 
830  J_A[0] = loc0 + loc3*matr[3] + loc4*dg[3];
831  J_A[1] = loc3*matr[4] + loc4*dg[4];
832  J_B[0] = loc3*matr[6] + loc4*dg[6];
833  J_B[1] = loc3*matr[7] + loc4*dg[7];
834 
835  loc3 = matr[4]*f + dg[4]*cross;
836  loc4 = dg[4]*g + matr[4]*cross;
837 
838  J_A[3] = loc0 + loc3*matr[4] + loc4*dg[4];
839  J_B[3] = loc3*matr[6] + loc4*dg[6];
840  J_B[4] = loc3*matr[7] + loc4*dg[7];
841 
842  /* Second diagonal block */
843  J_A[0] *= scale[0];
844  J_A[1] *= scale[1];
845  J_A[3] *= scale[2];
846 
847  A[0] = -J_A[0] - J_A[1];
848  A[4] = -J_A[1] - J_A[3];
849 
850  h_obj[0][1][1] = -A[0] - A[4];
851  h_obj[1][1][1] = A[0];
852  h_obj[2][1][1] = A[4];
853 
854  h_obj[3][1][1] = J_A[0];
855  h_obj[4][1][1] = J_A[1];
856 
857  h_obj[5][1][1] = J_A[3];
858 
859  /* Third off-diagonal block */
860  loc2 = matr[2]*loc1;
861  J_B[1] += loc2;
862  J_B[3] -= loc2;
863 
864  J_B[0] *= scale[0];
865  J_B[1] *= scale[1];
866  J_B[3] *= scale[1];
867  J_B[4] *= scale[2];
868 
869  A[0] = -J_B[0] - J_B[3];
870  A[4] = -J_B[1] - J_B[4];
871 
872  h_obj[0][1][2] = -A[0] - A[4];
873  h_obj[1][1][2] = A[0];
874  h_obj[2][1][2] = A[4];
875 
876  h_obj[1][2][1] = -J_B[0] - J_B[1];
877  h_obj[3][1][2] = J_B[0];
878  h_obj[4][1][2] = J_B[1];
879 
880  h_obj[2][2][1] = -J_B[3] - J_B[4];
881  h_obj[4][2][1] = J_B[3];
882  h_obj[5][1][2] = J_B[4];
883 
884  /* Third block of rows */
885  loc3 = matr[6]*f + dg[6]*cross;
886  loc4 = dg[6]*g + matr[6]*cross;
887 
888  J_A[0] = loc0 + loc3*matr[6] + loc4*dg[6];
889  J_A[1] = loc3*matr[7] + loc4*dg[7];
890 
891  loc3 = matr[7]*f + dg[7]*cross;
892  loc4 = dg[7]*g + matr[7]*cross;
893 
894  J_A[3] = loc0 + loc3*matr[7] + loc4*dg[7];
895 
896  /* Third diagonal block */
897  J_A[0] *= scale[0];
898  J_A[1] *= scale[1];
899  J_A[3] *= scale[2];
900 
901  A[0] = -J_A[0] - J_A[1];
902  A[4] = -J_A[1] - J_A[3];
903 
904  h_obj[0][2][2] = -A[0] - A[4];
905  h_obj[1][2][2] = A[0];
906  h_obj[2][2][2] = A[4];
907 
908  h_obj[3][2][2] = J_A[0];
909  h_obj[4][2][2] = J_A[1];
910 
911  h_obj[5][2][2] = J_A[3];
912 
913  // completes diagonal blocks.
914  h_obj[0].fill_lower_triangle();
915  h_obj[3].fill_lower_triangle();
916  h_obj[5].fill_lower_triangle();
917  return true;
918 }
919 
920 /*****************************************************************************/
921 /* The following set of functions reference tetrahedral elements to a */
922 /* regular tetrahedron. They are used when assessing the quality of a */
923 /* tetrahedral element. A zero return value indicates success, while */
924 /* a nonzero value indicates failure. */
925 /*****************************************************************************/
926 
927 /*****************************************************************************/
928 /* Function evaluation requires 62 flops. */
929 /* Reductions possible when b == 1 or c == 1 */
930 /*****************************************************************************/
931 inline bool m_fcn_3e(double &obj, const Vector3D x[4],
932  const double a, const Exponent& b, const Exponent& c)
933 {
934  double matr[9], f;
935  double g;
936 
937  /* Calculate M = A*inv(W). */
938  f = x[1][0] + x[0][0];
939  matr[0] = x[1][0] - x[0][0];
940  matr[1] = (2.0*x[2][0] - f)*isqrt3;
941  matr[2] = (3.0*x[3][0] - x[2][0] - f)*isqrt6;
942 
943  f = x[1][1] + x[0][1];
944  matr[3] = x[1][1] - x[0][1];
945  matr[4] = (2.0*x[2][1] - f)*isqrt3;
946  matr[5] = (3.0*x[3][1] - x[2][1] - f)*isqrt6;
947 
948  f = x[1][2] + x[0][2];
949  matr[6] = x[1][2] - x[0][2];
950  matr[7] = (2.0*x[2][2] - f)*isqrt3;
951  matr[8] = (3.0*x[3][2] - x[2][2] - f)*isqrt6;
952 
953  /* Calculate det(M). */
954  g = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
955  matr[1]*(matr[5]*matr[6] - matr[3]*matr[8]) +
956  matr[2]*(matr[3]*matr[7] - matr[4]*matr[6]);
957  if (g < MSQ_MIN) { obj = g; return false; }
958 
959  /* Calculate norm(M). */
960  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
961  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
962  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
963 
964  /* Calculate objective function. */
965  obj = a * pow(f, b) * pow(g, c);
966  return true;
967 }
968 
969 /*****************************************************************************/
970 /* Gradient evaluation requires 133 flops. */
971 /* Reductions possible when b == 1 or c == 1 */
972 /*****************************************************************************/
973 inline bool g_fcn_3e(double &obj, Vector3D g_obj[4], const Vector3D x[4],
974  const double a, const Exponent& b, const Exponent& c)
975 {
976  double matr[9], f;
977  double adj_m[9], g;
978  double loc1, loc2, loc3;
979 
980  /* Calculate M = A*inv(W). */
981  f = x[1][0] + x[0][0];
982  matr[0] = x[1][0] - x[0][0];
983  matr[1] = (2.0*x[2][0] - f)*isqrt3;
984  matr[2] = (3.0*x[3][0] - x[2][0] - f)*isqrt6;
985 
986  f = x[1][1] + x[0][1];
987  matr[3] = x[1][1] - x[0][1];
988  matr[4] = (2.0*x[2][1] - f)*isqrt3;
989  matr[5] = (3.0*x[3][1] - x[2][1] - f)*isqrt6;
990 
991  f = x[1][2] + x[0][2];
992  matr[6] = x[1][2] - x[0][2];
993  matr[7] = (2.0*x[2][2] - f)*isqrt3;
994  matr[8] = (3.0*x[3][2] - x[2][2] - f)*isqrt6;
995 
996  /* Calculate det(M). */
997  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
998  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
999  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
1000  g = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
1001  if (g < MSQ_MIN) { obj = g; return false; }
1002 
1003  /* Calculate norm(M). */
1004  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
1005  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
1006  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
1007 
1008  /* Calculate objective function. */
1009  obj = a * pow(f, b) * pow(g, c);
1010 
1011  /* Calculate the derivative of the objective function. */
1012  f = b * obj / f; /* Constant on nabla f */
1013  g = c * obj / g; /* Constant on nable g */
1014  f *= 2.0; /* Modification for nabla f */
1015 
1016  adj_m[0] = matr[0]*f + loc1*g;
1017  adj_m[1] = matr[1]*f + loc2*g;
1018  adj_m[2] = matr[2]*f + loc3*g;
1019 
1020  loc1 = matr[0]*g;
1021  loc2 = matr[1]*g;
1022  loc3 = matr[2]*g;
1023 
1024  adj_m[3] = matr[3]*f + loc3*matr[7] - loc2*matr[8];
1025  adj_m[4] = matr[4]*f + loc1*matr[8] - loc3*matr[6];
1026  adj_m[5] = matr[5]*f + loc2*matr[6] - loc1*matr[7];
1027 
1028  adj_m[6] = matr[6]*f + loc2*matr[5] - loc3*matr[4];
1029  adj_m[7] = matr[7]*f + loc3*matr[3] - loc1*matr[5];
1030  adj_m[8] = matr[8]*f + loc1*matr[4] - loc2*matr[3];
1031 
1032  loc1 = isqrt3*adj_m[1];
1033  loc2 = isqrt6*adj_m[2];
1034  loc3 = loc1 + loc2;
1035  g_obj[0][0] = -adj_m[0] - loc3;
1036  g_obj[1][0] = adj_m[0] - loc3;
1037  g_obj[2][0] = 2.0*loc1 - loc2;
1038  g_obj[3][0] = 3.0*loc2;
1039 
1040  loc1 = isqrt3*adj_m[4];
1041  loc2 = isqrt6*adj_m[5];
1042  loc3 = loc1 + loc2;
1043  g_obj[0][1] = -adj_m[3] - loc3;
1044  g_obj[1][1] = adj_m[3] - loc3;
1045  g_obj[2][1] = 2.0*loc1 - loc2;
1046  g_obj[3][1] = 3.0*loc2;
1047 
1048  loc1 = isqrt3*adj_m[7];
1049  loc2 = isqrt6*adj_m[8];
1050  loc3 = loc1 + loc2;
1051  g_obj[0][2] = -adj_m[6] - loc3;
1052  g_obj[1][2] = adj_m[6] - loc3;
1053  g_obj[2][2] = 2.0*loc1 - loc2;
1054  g_obj[3][2] = 3.0*loc2;
1055  return true;
1056 }
1057 
1058 /*****************************************************************************/
1059 /* Hessian evaluation requires 634 flops. */
1060 /* Reductions possible when b == 1 or c == 1 */
1061 /*****************************************************************************/
1062 inline bool h_fcn_3e(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10],
1063  const Vector3D x[4],
1064  const double a, const Exponent& b, const Exponent& c)
1065 {
1066 
1067  std::cout << "";
1068 
1069  double matr[9], f;
1070  double adj_m[9], g;
1071  double dg[9], loc0, loc1, loc2, loc3, loc4;
1072  double A[12], J_A[6], J_B[9], J_C[9], cross;
1073 
1074  /* Calculate M = A*inv(W). */
1075  f = x[1][0] + x[0][0];
1076  matr[0] = x[1][0] - x[0][0];
1077  matr[1] = (2.0*x[2][0] - f)*isqrt3;
1078  matr[2] = (3.0*x[3][0] - x[2][0] - f)*isqrt6;
1079 
1080  f = x[1][1] + x[0][1];
1081  matr[3] = x[1][1] - x[0][1];
1082  matr[4] = (2.0*x[2][1] - f)*isqrt3;
1083  matr[5] = (3.0*x[3][1] - x[2][1] - f)*isqrt6;
1084 
1085  f = x[1][2] + x[0][2];
1086  matr[6] = x[1][2] - x[0][2];
1087  matr[7] = (2.0*x[2][2] - f)*isqrt3;
1088  matr[8] = (3.0*x[3][2] - x[2][2] - f)*isqrt6;
1089 
1090  /* Calculate det(M). */
1091  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
1092  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
1093  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
1094  g = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
1095  if (g < MSQ_MIN) { obj = g; return false; }
1096 
1097  /* Calculate norm(M). */
1098  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
1099  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
1100  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
1101 
1102  loc3 = f;
1103  loc4 = g;
1104 
1105  /* Calculate objective function. */
1106  obj = a * pow(f, b) * pow(g, c);
1107 
1108  /* Calculate the derivative of the objective function. */
1109  f = b * obj / f; /* Constant on nabla f */
1110  g = c * obj / g; /* Constant on nable g */
1111  f *= 2.0; /* Modification for nabla f */
1112 
1113  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
1114  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
1115  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
1116  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
1117  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
1118  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
1119 
1120  adj_m[0] = matr[0]*f + dg[0]*g;
1121  adj_m[1] = matr[1]*f + dg[1]*g;
1122  adj_m[2] = matr[2]*f + dg[2]*g;
1123  adj_m[3] = matr[3]*f + dg[3]*g;
1124  adj_m[4] = matr[4]*f + dg[4]*g;
1125  adj_m[5] = matr[5]*f + dg[5]*g;
1126  adj_m[6] = matr[6]*f + dg[6]*g;
1127  adj_m[7] = matr[7]*f + dg[7]*g;
1128  adj_m[8] = matr[8]*f + dg[8]*g;
1129 
1130  loc0 = isqrt3*adj_m[1];
1131  loc1 = isqrt6*adj_m[2];
1132  loc2 = loc0 + loc1;
1133  g_obj[0][0] = -adj_m[0] - loc2;
1134  g_obj[1][0] = adj_m[0] - loc2;
1135  g_obj[2][0] = 2.0*loc0 - loc1;
1136  g_obj[3][0] = 3.0*loc1;
1137 
1138  loc0 = isqrt3*adj_m[4];
1139  loc1 = isqrt6*adj_m[5];
1140  loc2 = loc0 + loc1;
1141  g_obj[0][1] = -adj_m[3] - loc2;
1142  g_obj[1][1] = adj_m[3] - loc2;
1143  g_obj[2][1] = 2.0*loc0 - loc1;
1144  g_obj[3][1] = 3.0*loc1;
1145 
1146  loc0 = isqrt3*adj_m[7];
1147  loc1 = isqrt6*adj_m[8];
1148  loc2 = loc0 + loc1;
1149  g_obj[0][2] = -adj_m[6] - loc2;
1150  g_obj[1][2] = adj_m[6] - loc2;
1151  g_obj[2][2] = 2.0*loc0 - loc1;
1152  g_obj[3][2] = 3.0*loc1;
1153 
1154  /* Calculate the hessian of the objective. */
1155  loc0 = f; /* Constant on nabla^2 f */
1156  loc1 = g; /* Constant on nabla^2 g */
1157  cross = f * c / loc4; /* Constant on nabla g nabla f */
1158  f = f * (b-1) / loc3; /* Constant on nabla f nabla f */
1159  g = g * (c-1) / loc4; /* Constant on nabla g nabla g */
1160  f *= 2.0; /* Modification for nabla f */
1161 
1162  /* First block of rows */
1163  loc3 = matr[0]*f + dg[0]*cross;
1164  loc4 = dg[0]*g + matr[0]*cross;
1165 
1166  J_A[0] = loc0 + loc3*matr[0] + loc4*dg[0];
1167  J_A[1] = loc3*matr[1] + loc4*dg[1];
1168  J_A[2] = loc3*matr[2] + loc4*dg[2];
1169  J_B[0] = loc3*matr[3] + loc4*dg[3];
1170  J_B[1] = loc3*matr[4] + loc4*dg[4];
1171  J_B[2] = loc3*matr[5] + loc4*dg[5];
1172  J_C[0] = loc3*matr[6] + loc4*dg[6];
1173  J_C[1] = loc3*matr[7] + loc4*dg[7];
1174  J_C[2] = loc3*matr[8] + loc4*dg[8];
1175 
1176  loc3 = matr[1]*f + dg[1]*cross;
1177  loc4 = dg[1]*g + matr[1]*cross;
1178 
1179  J_A[3] = loc0 + loc3*matr[1] + loc4*dg[1];
1180  J_A[4] = loc3*matr[2] + loc4*dg[2];
1181  J_B[3] = loc3*matr[3] + loc4*dg[3];
1182  J_B[4] = loc3*matr[4] + loc4*dg[4];
1183  J_B[5] = loc3*matr[5] + loc4*dg[5];
1184  J_C[3] = loc3*matr[6] + loc4*dg[6];
1185  J_C[4] = loc3*matr[7] + loc4*dg[7];
1186  J_C[5] = loc3*matr[8] + loc4*dg[8];
1187 
1188  loc3 = matr[2]*f + dg[2]*cross;
1189  loc4 = dg[2]*g + matr[2]*cross;
1190 
1191  J_A[5] = loc0 + loc3*matr[2] + loc4*dg[2];
1192  J_B[6] = loc3*matr[3] + loc4*dg[3];
1193  J_B[7] = loc3*matr[4] + loc4*dg[4];
1194  J_B[8] = loc3*matr[5] + loc4*dg[5];
1195  J_C[6] = loc3*matr[6] + loc4*dg[6];
1196  J_C[7] = loc3*matr[7] + loc4*dg[7];
1197  J_C[8] = loc3*matr[8] + loc4*dg[8];
1198 
1199  /* First diagonal block */
1200  loc2 = isqrt3*J_A[1];
1201  loc3 = isqrt6*J_A[2];
1202  loc4 = loc2 + loc3;
1203 
1204  A[0] = -J_A[0] - loc4;
1205  A[1] = J_A[0] - loc4;
1206 
1207  loc2 = isqrt3*J_A[3];
1208  loc3 = isqrt6*J_A[4];
1209  loc4 = loc2 + loc3;
1210 
1211  A[4] = -J_A[1] - loc4;
1212  A[5] = J_A[1] - loc4;
1213  A[6] = 2.0*loc2 - loc3;
1214 
1215  loc2 = isqrt3*J_A[4];
1216  loc3 = isqrt6*J_A[5];
1217  loc4 = loc2 + loc3;
1218 
1219  A[8] = -J_A[2] - loc4;
1220  A[9] = J_A[2] - loc4;
1221  A[10] = 2.0*loc2 - loc3;
1222  A[11] = 3.0*loc3;
1223 
1224  loc2 = isqrt3*A[4];
1225  loc3 = isqrt6*A[8];
1226  loc4 = loc2 + loc3;
1227 
1228  h_obj[0][0][0] = -A[0] - loc4;
1229  h_obj[1][0][0] = A[0] - loc4;
1230  h_obj[2][0][0] = 2.0*loc2 - loc3;
1231  h_obj[3][0][0] = 3.0*loc3;
1232 
1233  loc2 = isqrt3*A[5];
1234  loc3 = isqrt6*A[9];
1235 
1236  h_obj[4][0][0] = A[1] - loc2 - loc3;
1237  h_obj[5][0][0] = 2.0*loc2 - loc3;
1238  h_obj[6][0][0] = 3.0*loc3;
1239 
1240  loc3 = isqrt6*A[10];
1241  h_obj[7][0][0] = tisqrt3*A[6] - loc3;
1242  h_obj[8][0][0] = 3.0*loc3;
1243 
1244  h_obj[9][0][0] = tisqrt6*A[11];
1245 
1246  /* First off-diagonal block */
1247  loc2 = matr[8]*loc1;
1248  J_B[1] += loc2;
1249  J_B[3] -= loc2;
1250 
1251  loc2 = matr[7]*loc1;
1252  J_B[2] -= loc2;
1253  J_B[6] += loc2;
1254 
1255  loc2 = matr[6]*loc1;
1256  J_B[5] += loc2;
1257  J_B[7] -= loc2;
1258 
1259  loc2 = isqrt3*J_B[3];
1260  loc3 = isqrt6*J_B[6];
1261  loc4 = loc2 + loc3;
1262 
1263  A[0] = -J_B[0] - loc4;
1264  A[1] = J_B[0] - loc4;
1265  A[2] = 2.0*loc2 - loc3;
1266  A[3] = 3.0*loc3;
1267 
1268  loc2 = isqrt3*J_B[4];
1269  loc3 = isqrt6*J_B[7];
1270  loc4 = loc2 + loc3;
1271 
1272  A[4] = -J_B[1] - loc4;
1273  A[5] = J_B[1] - loc4;
1274  A[6] = 2.0*loc2 - loc3;
1275  A[7] = 3.0*loc3;
1276 
1277  loc2 = isqrt3*J_B[5];
1278  loc3 = isqrt6*J_B[8];
1279  loc4 = loc2 + loc3;
1280 
1281  A[8] = -J_B[2] - loc4;
1282  A[9] = J_B[2] - loc4;
1283  A[10] = 2.0*loc2 - loc3;
1284  A[11] = 3.0*loc3;
1285 
1286  loc2 = isqrt3*A[4];
1287  loc3 = isqrt6*A[8];
1288  loc4 = loc2 + loc3;
1289 
1290  h_obj[0][0][1] = -A[0] - loc4;
1291  h_obj[1][0][1] = A[0] - loc4;
1292  h_obj[2][0][1] = 2.0*loc2 - loc3;
1293  h_obj[3][0][1] = 3.0*loc3;
1294 
1295  loc2 = isqrt3*A[5];
1296  loc3 = isqrt6*A[9];
1297  loc4 = loc2 + loc3;
1298 
1299  h_obj[1][1][0] = -A[1] - loc4;
1300  h_obj[4][0][1] = A[1] - loc4;
1301  h_obj[5][0][1] = 2.0*loc2 - loc3;
1302  h_obj[6][0][1] = 3.0*loc3;
1303 
1304  loc2 = isqrt3*A[6];
1305  loc3 = isqrt6*A[10];
1306  loc4 = loc2 + loc3;
1307 
1308  h_obj[2][1][0] = -A[2] - loc4;
1309  h_obj[5][1][0] = A[2] - loc4;
1310  h_obj[7][0][1] = 2.0*loc2 - loc3;
1311  h_obj[8][0][1] = 3.0*loc3;
1312 
1313  loc2 = isqrt3*A[7];
1314  loc3 = isqrt6*A[11];
1315  loc4 = loc2 + loc3;
1316 
1317  h_obj[3][1][0] = -A[3] - loc4;
1318  h_obj[6][1][0] = A[3] - loc4;
1319  h_obj[8][1][0] = 2.0*loc2 - loc3;
1320  h_obj[9][0][1] = 3.0*loc3;
1321 
1322  /* Second off-diagonal block */
1323  loc2 = matr[5]*loc1;
1324  J_C[1] -= loc2;
1325  J_C[3] += loc2;
1326 
1327  loc2 = matr[4]*loc1;
1328  J_C[2] += loc2;
1329  J_C[6] -= loc2;
1330 
1331  loc2 = matr[3]*loc1;
1332  J_C[5] -= loc2;
1333  J_C[7] += loc2;
1334 
1335  loc2 = isqrt3*J_C[3];
1336  loc3 = isqrt6*J_C[6];
1337  loc4 = loc2 + loc3;
1338 
1339  A[0] = -J_C[0] - loc4;
1340  A[1] = J_C[0] - loc4;
1341  A[2] = 2.0*loc2 - loc3;
1342  A[3] = 3.0*loc3;
1343 
1344  loc2 = isqrt3*J_C[4];
1345  loc3 = isqrt6*J_C[7];
1346  loc4 = loc2 + loc3;
1347 
1348  A[4] = -J_C[1] - loc4;
1349  A[5] = J_C[1] - loc4;
1350  A[6] = 2.0*loc2 - loc3;
1351  A[7] = 3.0*loc3;
1352 
1353  loc2 = isqrt3*J_C[5];
1354  loc3 = isqrt6*J_C[8];
1355  loc4 = loc2 + loc3;
1356 
1357  A[8] = -J_C[2] - loc4;
1358  A[9] = J_C[2] - loc4;
1359  A[10] = 2.0*loc2 - loc3;
1360  A[11] = 3.0*loc3;
1361 
1362  loc2 = isqrt3*A[4];
1363  loc3 = isqrt6*A[8];
1364  loc4 = loc2 + loc3;
1365 
1366  h_obj[0][0][2] = -A[0] - loc4;
1367  h_obj[1][0][2] = A[0] - loc4;
1368  h_obj[2][0][2] = 2.0*loc2 - loc3;
1369  h_obj[3][0][2] = 3.0*loc3;
1370 
1371  loc2 = isqrt3*A[5];
1372  loc3 = isqrt6*A[9];
1373  loc4 = loc2 + loc3;
1374 
1375  h_obj[1][2][0] = -A[1] - loc4;
1376  h_obj[4][0][2] = A[1] - loc4;
1377  h_obj[5][0][2] = 2.0*loc2 - loc3;
1378  h_obj[6][0][2] = 3.0*loc3;
1379 
1380  loc2 = isqrt3*A[6];
1381  loc3 = isqrt6*A[10];
1382  loc4 = loc2 + loc3;
1383 
1384  h_obj[2][2][0] = -A[2] - loc4;
1385  h_obj[5][2][0] = A[2] - loc4;
1386  h_obj[7][0][2] = 2.0*loc2 - loc3;
1387  h_obj[8][0][2] = 3.0*loc3;
1388 
1389  loc2 = isqrt3*A[7];
1390  loc3 = isqrt6*A[11];
1391  loc4 = loc2 + loc3;
1392 
1393  h_obj[3][2][0] = -A[3] - loc4;
1394  h_obj[6][2][0] = A[3] - loc4;
1395  h_obj[8][2][0] = 2.0*loc2 - loc3;
1396  h_obj[9][0][2] = 3.0*loc3;
1397 
1398  /* Second block of rows */
1399  loc3 = matr[3]*f + dg[3]*cross;
1400  loc4 = dg[3]*g + matr[3]*cross;
1401 
1402  J_A[0] = loc0 + loc3*matr[3] + loc4*dg[3];
1403  J_A[1] = loc3*matr[4] + loc4*dg[4];
1404  J_A[2] = loc3*matr[5] + loc4*dg[5];
1405  J_B[0] = loc3*matr[6] + loc4*dg[6];
1406  J_B[1] = loc3*matr[7] + loc4*dg[7];
1407  J_B[2] = loc3*matr[8] + loc4*dg[8];
1408 
1409  loc3 = matr[4]*f + dg[4]*cross;
1410  loc4 = dg[4]*g + matr[4]*cross;
1411 
1412  J_A[3] = loc0 + loc3*matr[4] + loc4*dg[4];
1413  J_A[4] = loc3*matr[5] + loc4*dg[5];
1414  J_B[3] = loc3*matr[6] + loc4*dg[6];
1415  J_B[4] = loc3*matr[7] + loc4*dg[7];
1416  J_B[5] = loc3*matr[8] + loc4*dg[8];
1417 
1418  loc3 = matr[5]*f + dg[5]*cross;
1419  loc4 = dg[5]*g + matr[5]*cross;
1420 
1421  J_A[5] = loc0 + loc3*matr[5] + loc4*dg[5];
1422  J_B[6] = loc3*matr[6] + loc4*dg[6];
1423  J_B[7] = loc3*matr[7] + loc4*dg[7];
1424  J_B[8] = loc3*matr[8] + loc4*dg[8];
1425 
1426  /* Second diagonal block */
1427  loc2 = isqrt3*J_A[1];
1428  loc3 = isqrt6*J_A[2];
1429  loc4 = loc2 + loc3;
1430 
1431  A[0] = -J_A[0] - loc4;
1432  A[1] = J_A[0] - loc4;
1433 
1434  loc2 = isqrt3*J_A[3];
1435  loc3 = isqrt6*J_A[4];
1436  loc4 = loc2 + loc3;
1437 
1438  A[4] = -J_A[1] - loc4;
1439  A[5] = J_A[1] - loc4;
1440  A[6] = 2.0*loc2 - loc3;
1441 
1442  loc2 = isqrt3*J_A[4];
1443  loc3 = isqrt6*J_A[5];
1444  loc4 = loc2 + loc3;
1445 
1446  A[8] = -J_A[2] - loc4;
1447  A[9] = J_A[2] - loc4;
1448  A[10] = 2.0*loc2 - loc3;
1449  A[11] = 3.0*loc3;
1450 
1451  loc2 = isqrt3*A[4];
1452  loc3 = isqrt6*A[8];
1453  loc4 = loc2 + loc3;
1454 
1455  h_obj[0][1][1] = -A[0] - loc4;
1456  h_obj[1][1][1] = A[0] - loc4;
1457  h_obj[2][1][1] = 2.0*loc2 - loc3;
1458  h_obj[3][1][1] = 3.0*loc3;
1459 
1460  loc2 = isqrt3*A[5];
1461  loc3 = isqrt6*A[9];
1462 
1463  h_obj[4][1][1] = A[1] - loc2 - loc3;
1464  h_obj[5][1][1] = 2.0*loc2 - loc3;
1465  h_obj[6][1][1] = 3.0*loc3;
1466 
1467  loc3 = isqrt6*A[10];
1468  h_obj[7][1][1] = tisqrt3*A[6] - loc3;
1469  h_obj[8][1][1] = 3.0*loc3;
1470 
1471  h_obj[9][1][1] = tisqrt6*A[11];
1472 
1473  /* Third off-diagonal block */
1474  loc2 = matr[2]*loc1;
1475  J_B[1] += loc2;
1476  J_B[3] -= loc2;
1477 
1478  loc2 = matr[1]*loc1;
1479  J_B[2] -= loc2;
1480  J_B[6] += loc2;
1481 
1482  loc2 = matr[0]*loc1;
1483  J_B[5] += loc2;
1484  J_B[7] -= loc2;
1485 
1486  loc2 = isqrt3*J_B[3];
1487  loc3 = isqrt6*J_B[6];
1488  loc4 = loc2 + loc3;
1489 
1490  A[0] = -J_B[0] - loc4;
1491  A[1] = J_B[0] - loc4;
1492  A[2] = 2.0*loc2 - loc3;
1493  A[3] = 3.0*loc3;
1494 
1495  loc2 = isqrt3*J_B[4];
1496  loc3 = isqrt6*J_B[7];
1497  loc4 = loc2 + loc3;
1498 
1499  A[4] = -J_B[1] - loc4;
1500  A[5] = J_B[1] - loc4;
1501  A[6] = 2.0*loc2 - loc3;
1502  A[7] = 3.0*loc3;
1503 
1504  loc2 = isqrt3*J_B[5];
1505  loc3 = isqrt6*J_B[8];
1506  loc4 = loc2 + loc3;
1507 
1508  A[8] = -J_B[2] - loc4;
1509  A[9] = J_B[2] - loc4;
1510  A[10] = 2.0*loc2 - loc3;
1511  A[11] = 3.0*loc3;
1512 
1513  loc2 = isqrt3*A[4];
1514  loc3 = isqrt6*A[8];
1515  loc4 = loc2 + loc3;
1516 
1517  h_obj[0][1][2] = -A[0] - loc4;
1518  h_obj[1][1][2] = A[0] - loc4;
1519  h_obj[2][1][2] = 2.0*loc2 - loc3;
1520  h_obj[3][1][2] = 3.0*loc3;
1521 
1522  loc2 = isqrt3*A[5];
1523  loc3 = isqrt6*A[9];
1524  loc4 = loc2 + loc3;
1525 
1526  h_obj[1][2][1] = -A[1] - loc4;
1527  h_obj[4][1][2] = A[1] - loc4;
1528  h_obj[5][1][2] = 2.0*loc2 - loc3;
1529  h_obj[6][1][2] = 3.0*loc3;
1530 
1531  loc2 = isqrt3*A[6];
1532  loc3 = isqrt6*A[10];
1533  loc4 = loc2 + loc3;
1534 
1535  h_obj[2][2][1] = -A[2] - loc4;
1536  h_obj[5][2][1] = A[2] - loc4;
1537  h_obj[7][1][2] = 2.0*loc2 - loc3;
1538  h_obj[8][1][2] = 3.0*loc3;
1539 
1540  loc2 = isqrt3*A[7];
1541  loc3 = isqrt6*A[11];
1542  loc4 = loc2 + loc3;
1543 
1544  h_obj[3][2][1] = -A[3] - loc4;
1545  h_obj[6][2][1] = A[3] - loc4;
1546  h_obj[8][2][1] = 2.0*loc2 - loc3;
1547  h_obj[9][1][2] = 3.0*loc3;
1548 
1549  /* Third block of rows */
1550  loc3 = matr[6]*f + dg[6]*cross;
1551  loc4 = dg[6]*g + matr[6]*cross;
1552 
1553  J_A[0] = loc0 + loc3*matr[6] + loc4*dg[6];
1554  J_A[1] = loc3*matr[7] + loc4*dg[7];
1555  J_A[2] = loc3*matr[8] + loc4*dg[8];
1556 
1557  loc3 = matr[7]*f + dg[7]*cross;
1558  loc4 = dg[7]*g + matr[7]*cross;
1559 
1560  J_A[3] = loc0 + loc3*matr[7] + loc4*dg[7];
1561  J_A[4] = loc3*matr[8] + loc4*dg[8];
1562 
1563  loc3 = matr[8]*f + dg[8]*cross;
1564  loc4 = dg[8]*g + matr[8]*cross;
1565 
1566  J_A[5] = loc0 + loc3*matr[8] + loc4*dg[8];
1567 
1568  /* Third diagonal block */
1569  loc2 = isqrt3*J_A[1];
1570  loc3 = isqrt6*J_A[2];
1571  loc4 = loc2 + loc3;
1572 
1573  A[0] = -J_A[0] - loc4;
1574  A[1] = J_A[0] - loc4;
1575 
1576  loc2 = isqrt3*J_A[3];
1577  loc3 = isqrt6*J_A[4];
1578  loc4 = loc2 + loc3;
1579 
1580  A[4] = -J_A[1] - loc4;
1581  A[5] = J_A[1] - loc4;
1582  A[6] = 2.0*loc2 - loc3;
1583 
1584  loc2 = isqrt3*J_A[4];
1585  loc3 = isqrt6*J_A[5];
1586  loc4 = loc2 + loc3;
1587 
1588  A[8] = -J_A[2] - loc4;
1589  A[9] = J_A[2] - loc4;
1590  A[10] = 2.0*loc2 - loc3;
1591  A[11] = 3.0*loc3;
1592 
1593  loc2 = isqrt3*A[4];
1594  loc3 = isqrt6*A[8];
1595  loc4 = loc2 + loc3;
1596 
1597  h_obj[0][2][2] = -A[0] - loc4;
1598  h_obj[1][2][2] = A[0] - loc4;
1599  h_obj[2][2][2] = 2.0*loc2 - loc3;
1600  h_obj[3][2][2] = 3.0*loc3;
1601 
1602  loc2 = isqrt3*A[5];
1603  loc3 = isqrt6*A[9];
1604 
1605  h_obj[4][2][2] = A[1] - loc2 - loc3;
1606  h_obj[5][2][2] = 2.0*loc2 - loc3;
1607  h_obj[6][2][2] = 3.0*loc3;
1608 
1609  loc3 = isqrt6*A[10];
1610  h_obj[7][2][2] = tisqrt3*A[6] - loc3;
1611  h_obj[8][2][2] = 3.0*loc3;
1612 
1613  h_obj[9][2][2] = tisqrt6*A[11];
1614 
1615  // completes diagonal blocks.
1616  h_obj[0].fill_lower_triangle();
1617  h_obj[4].fill_lower_triangle();
1618  h_obj[7].fill_lower_triangle();
1619  h_obj[9].fill_lower_triangle();
1620  return true;
1621 }
1622 
1623 /*****************************************************************************/
1624 /* The following set of functions reference tetrahedral elements to a */
1625 /* equilateral tetrahedron. These functions are specialized toward */
1626 /* obtaining the gradient and Hessian with respect to a single vertex. */
1627 /*****************************************************************************/
1628 
1629 inline bool g_fcn_3e_v3(double &obj, Vector3D &g_obj, const Vector3D x[4],
1630  const double a, const Exponent& b, const Exponent& c)
1631 {
1632  double matr[9], f, g;
1633  double loc1, loc2, loc3;
1634 
1635  /* Calculate M = A*inv(W). */
1636  f = x[1][0] + x[0][0];
1637  matr[0] = x[1][0] - x[0][0];
1638  matr[1] = (2.0*x[2][0] - f)*isqrt3;
1639  matr[2] = (3.0*x[3][0] - x[2][0] - f)*isqrt6;
1640 
1641  f = x[1][1] + x[0][1];
1642  matr[3] = x[1][1] - x[0][1];
1643  matr[4] = (2.0*x[2][1] - f)*isqrt3;
1644  matr[5] = (3.0*x[3][1] - x[2][1] - f)*isqrt6;
1645 
1646  f = x[1][2] + x[0][2];
1647  matr[6] = x[1][2] - x[0][2];
1648  matr[7] = (2.0*x[2][2] - f)*isqrt3;
1649  matr[8] = (3.0*x[3][2] - x[2][2] - f)*isqrt6;
1650 
1651  /* Calculate det(M). */
1652  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
1653  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
1654  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
1655  g = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
1656  if (g < MSQ_MIN) { obj = g; return false; }
1657 
1658  /* Calculate norm(M). */
1659  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
1660  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
1661  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
1662 
1663  /* Calculate objective function. */
1664  obj = a * pow(f, b) * pow(g, c);
1665 
1666  /* Calculate the derivative of the objective function. */
1667  f = b * obj / f; /* Constant on nabla f */
1668  g = c * obj / g; /* Constant on nable g */
1669  f *= 2.0; /* Modification for nabla f */
1670 
1671  g_obj[0] = tisqrt6*(matr[2]*f + loc3*g);
1672 
1673  loc1 = matr[0]*g;
1674  loc2 = matr[1]*g;
1675 
1676  g_obj[1] = tisqrt6*(matr[5]*f + loc2*matr[6] - loc1*matr[7]);
1677  g_obj[2] = tisqrt6*(matr[8]*f + loc1*matr[4] - loc2*matr[3]);
1678  return true;
1679 }
1680 
1681 inline bool g_fcn_3e_v0(double &obj, Vector3D &g_obj, const Vector3D x[4],
1682  const double a, const Exponent& b, const Exponent& c)
1683 {
1684  static Vector3D my_x[4];
1685 
1686  my_x[0] = x[1];
1687  my_x[1] = x[3];
1688  my_x[2] = x[2];
1689  my_x[3] = x[0];
1690  return g_fcn_3e_v3(obj, g_obj, my_x, a, b, c);
1691 }
1692 
1693 inline bool g_fcn_3e_v1(double &obj, Vector3D &g_obj, const Vector3D x[4],
1694  const double a, const Exponent& b, const Exponent& c)
1695 {
1696  static Vector3D my_x[4];
1697 
1698  my_x[0] = x[0];
1699  my_x[1] = x[2];
1700  my_x[2] = x[3];
1701  my_x[3] = x[1];
1702  return g_fcn_3e_v3(obj, g_obj, my_x, a, b, c);
1703 }
1704 
1705 inline bool g_fcn_3e_v2(double &obj, Vector3D &g_obj, const Vector3D x[4],
1706  const double a, const Exponent& b, const Exponent& c)
1707 {
1708  static Vector3D my_x[4];
1709 
1710  my_x[0] = x[1];
1711  my_x[1] = x[0];
1712  my_x[2] = x[3];
1713  my_x[3] = x[2];
1714  return g_fcn_3e_v3(obj, g_obj, my_x, a, b, c);
1715 }
1716 
1717 inline bool h_fcn_3e_v3(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
1718  const Vector3D x[4],
1719  const double a, const Exponent& b, const Exponent& c)
1720 {
1721  double matr[9], f, g;
1722  double dg[9], loc0, loc1, loc3, loc4;
1723  double cross;
1724 
1725  /* Calculate M = A*inv(W). */
1726  f = x[1][0] + x[0][0];
1727  matr[0] = x[1][0] - x[0][0];
1728  matr[1] = (2.0*x[2][0] - f)*isqrt3;
1729  matr[2] = (3.0*x[3][0] - x[2][0] - f)*isqrt6;
1730 
1731  f = x[1][1] + x[0][1];
1732  matr[3] = x[1][1] - x[0][1];
1733  matr[4] = (2.0*x[2][1] - f)*isqrt3;
1734  matr[5] = (3.0*x[3][1] - x[2][1] - f)*isqrt6;
1735 
1736  f = x[1][2] + x[0][2];
1737  matr[6] = x[1][2] - x[0][2];
1738  matr[7] = (2.0*x[2][2] - f)*isqrt3;
1739  matr[8] = (3.0*x[3][2] - x[2][2] - f)*isqrt6;
1740 
1741  /* Calculate det(M). */
1742  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
1743  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
1744  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
1745  g = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
1746  if (g < MSQ_MIN) { obj = g; return false; }
1747 
1748  /* Calculate norm(M). */
1749  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
1750  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
1751  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
1752 
1753  loc3 = f;
1754  loc4 = g;
1755 
1756  /* Calculate objective function. */
1757  obj = a * pow(f, b) * pow(g, c);
1758 
1759  /* Calculate the derivative of the objective function. */
1760  f = b * obj / f; /* Constant on nabla f */
1761  g = c * obj / g; /* Constant on nable g */
1762  f *= 2.0; /* Modification for nabla f */
1763 
1764  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
1765  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
1766 
1767  g_obj[0] = tisqrt6*(matr[2]*f + dg[2]*g);
1768  g_obj[1] = tisqrt6*(matr[5]*f + dg[5]*g);
1769  g_obj[2] = tisqrt6*(matr[8]*f + dg[8]*g);
1770 
1771  /* Calculate the hessian of the objective. */
1772  loc0 = f; /* Constant on nabla^2 f */
1773  loc1 = g; /* Constant on nabla^2 g */
1774  cross = f * c / loc4; /* Constant on nabla g nabla f */
1775  f = f * (b-1) / loc3; /* Constant on nabla f nabla f */
1776  g = g * (c-1) / loc4; /* Constant on nabla g nabla g */
1777  f *= 2.0; /* Modification for nabla f */
1778 
1779  /* First block of rows */
1780  loc3 = matr[2]*f + dg[2]*cross;
1781  loc4 = dg[2]*g + matr[2]*cross;
1782 
1783  h_obj[0][0] = 1.5*(loc0 + loc3*matr[2] + loc4*dg[2]);
1784  h_obj[0][1] = 1.5*(loc3*matr[5] + loc4*dg[5]);
1785  h_obj[0][2] = 1.5*(loc3*matr[8] + loc4*dg[8]);
1786 
1787  /* Second block of rows */
1788  loc3 = matr[5]*f + dg[5]*cross;
1789  loc4 = dg[5]*g + matr[5]*cross;
1790 
1791  h_obj[1][1] = 1.5*(loc0 + loc3*matr[5] + loc4*dg[5]);
1792  h_obj[1][2] = 1.5*(loc3*matr[8] + loc4*dg[8]);
1793 
1794  /* Third block of rows */
1795  loc3 = matr[8]*f + dg[8]*cross;
1796  loc4 = dg[8]*g + matr[8]*cross;
1797 
1798  h_obj[2][2] = 1.5*(loc0 + loc3*matr[8] + loc4*dg[8]);
1799 
1800  // completes diagonal block.
1801  h_obj.fill_lower_triangle();
1802  return true;
1803 }
1804 
1805 inline bool h_fcn_3e_v0(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
1806  const Vector3D x[4],
1807  const double a, const Exponent& b, const Exponent& c)
1808 {
1809  static Vector3D my_x[4];
1810 
1811  my_x[0] = x[1];
1812  my_x[1] = x[3];
1813  my_x[2] = x[2];
1814  my_x[3] = x[0];
1815  return h_fcn_3e_v3(obj, g_obj, h_obj, my_x, a, b, c);
1816 }
1817 
1818 inline bool h_fcn_3e_v1(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
1819  const Vector3D x[4],
1820  const double a, const Exponent& b, const Exponent& c)
1821 {
1822  static Vector3D my_x[4];
1823 
1824  my_x[0] = x[0];
1825  my_x[1] = x[2];
1826  my_x[2] = x[3];
1827  my_x[3] = x[1];
1828  return h_fcn_3e_v3(obj, g_obj, h_obj, my_x, a, b, c);
1829 }
1830 
1831 inline bool h_fcn_3e_v2(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
1832  const Vector3D x[4],
1833  const double a, const Exponent& b, const Exponent& c)
1834 {
1835  static Vector3D my_x[4];
1836 
1837  my_x[0] = x[1];
1838  my_x[1] = x[0];
1839  my_x[2] = x[3];
1840  my_x[3] = x[2];
1841  return h_fcn_3e_v3(obj, g_obj, h_obj, my_x, a, b, c);
1842 }
1843 
1844 /*****************************************************************************/
1845 /* The following set of functions reference tetrahedral elements to a */
1846 /* right tetrahedron. They are used when assessing the quality of a */
1847 /* hexahedral element. A zero return value indicates success, while */
1848 /* a nonzero value indicates failure. */
1849 /*****************************************************************************/
1850 
1851 /*****************************************************************************/
1852 /* Function evaluation requires 53 flops. */
1853 /* Reductions possible when b == 1, c == 1, or d == 1 */
1854 /*****************************************************************************/
1855 inline bool m_fcn_3i(double &obj, const Vector3D x[4],
1856  const double a, const Exponent& b, const Exponent& c,
1857  const Vector3D &d)
1858 {
1859  double matr[9], f;
1860  double g;
1861 
1862  /* Calculate M = A*inv(W). */
1863  matr[0] = d[0]*(x[1][0] - x[0][0]);
1864  matr[1] = d[1]*(x[2][0] - x[0][0]);
1865  matr[2] = d[2]*(x[3][0] - x[0][0]);
1866 
1867  matr[3] = d[0]*(x[1][1] - x[0][1]);
1868  matr[4] = d[1]*(x[2][1] - x[0][1]);
1869  matr[5] = d[2]*(x[3][1] - x[0][1]);
1870 
1871  matr[6] = d[0]*(x[1][2] - x[0][2]);
1872  matr[7] = d[1]*(x[2][2] - x[0][2]);
1873  matr[8] = d[2]*(x[3][2] - x[0][2]);
1874 
1875  /* Calculate det(M). */
1876  g = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
1877  matr[1]*(matr[5]*matr[6] - matr[3]*matr[8]) +
1878  matr[2]*(matr[3]*matr[7] - matr[4]*matr[6]);
1879  if (g < MSQ_MIN) { obj = g; return false; }
1880 
1881  /* Calculate norm(M). */
1882  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
1883  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
1884  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
1885 
1886  /* Calculate objective function. */
1887  obj = a * pow(f, b) * pow(g, c);
1888  return true;
1889 }
1890 
1891 /*****************************************************************************/
1892 /* Gradient evaluation requires 115 flops. */
1893 /* Reductions possible when b == 1, c == 1, or d == 1 */
1894 /*****************************************************************************/
1895 inline bool g_fcn_3i(double &obj, Vector3D g_obj[4], const Vector3D x[4],
1896  const double a, const Exponent& b, const Exponent& c,
1897  const Vector3D &d)
1898 {
1899  double matr[9], f;
1900  double adj_m[9], g;
1901  double loc1, loc2, loc3;
1902 
1903  /* Calculate M = A*inv(W). */
1904  matr[0] = d[0]*(x[1][0] - x[0][0]);
1905  matr[1] = d[1]*(x[2][0] - x[0][0]);
1906  matr[2] = d[2]*(x[3][0] - x[0][0]);
1907 
1908  matr[3] = d[0]*(x[1][1] - x[0][1]);
1909  matr[4] = d[1]*(x[2][1] - x[0][1]);
1910  matr[5] = d[2]*(x[3][1] - x[0][1]);
1911 
1912  matr[6] = d[0]*(x[1][2] - x[0][2]);
1913  matr[7] = d[1]*(x[2][2] - x[0][2]);
1914  matr[8] = d[2]*(x[3][2] - x[0][2]);
1915 
1916  /* Calculate det(M). */
1917  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
1918  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
1919  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
1920  g = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
1921  if (g < MSQ_MIN) { obj = g; return false; }
1922 
1923  /* Calculate norm(M). */
1924  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
1925  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
1926  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
1927 
1928  /* Calculate objective function. */
1929  obj = a * pow(f, b) * pow(g, c);
1930 
1931  /* Calculate the derivative of the objective function. */
1932  f = b * obj / f; /* Constant on nabla f */
1933  g = c * obj / g; /* Constant on nable g */
1934  f *= 2.0; /* Modification for nabla f */
1935 
1936  adj_m[0] = d[0]*(matr[0]*f + loc1*g);
1937  adj_m[1] = d[1]*(matr[1]*f + loc2*g);
1938  adj_m[2] = d[2]*(matr[2]*f + loc3*g);
1939 
1940  loc1 = matr[0]*g;
1941  loc2 = matr[1]*g;
1942  loc3 = matr[2]*g;
1943 
1944  adj_m[3] = d[0]*(matr[3]*f + loc3*matr[7] - loc2*matr[8]);
1945  adj_m[4] = d[1]*(matr[4]*f + loc1*matr[8] - loc3*matr[6]);
1946  adj_m[5] = d[2]*(matr[5]*f + loc2*matr[6] - loc1*matr[7]);
1947 
1948  adj_m[6] = d[0]*(matr[6]*f + loc2*matr[5] - loc3*matr[4]);
1949  adj_m[7] = d[1]*(matr[7]*f + loc3*matr[3] - loc1*matr[5]);
1950  adj_m[8] = d[2]*(matr[8]*f + loc1*matr[4] - loc2*matr[3]);
1951 
1952  g_obj[0][0] = -adj_m[0] - adj_m[1] - adj_m[2];
1953  g_obj[1][0] = adj_m[0];
1954  g_obj[2][0] = adj_m[1];
1955  g_obj[3][0] = adj_m[2];
1956 
1957  g_obj[0][1] = -adj_m[3] - adj_m[4] - adj_m[5];
1958  g_obj[1][1] = adj_m[3];
1959  g_obj[2][1] = adj_m[4];
1960  g_obj[3][1] = adj_m[5];
1961 
1962  g_obj[0][2] = -adj_m[6] - adj_m[7] - adj_m[8];
1963  g_obj[1][2] = adj_m[6];
1964  g_obj[2][2] = adj_m[7];
1965  g_obj[3][2] = adj_m[8];
1966  return true;
1967 }
1968 
1969 /*****************************************************************************/
1970 /* Hessian evaluation requires 469 flops. */
1971 /* Reductions possible when b == 1, c == 1, or d == 1 */
1972 /*****************************************************************************/
1973 inline int h_fcn_3i(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10],
1974  const Vector3D x[4],
1975  const double a, const Exponent& b, const Exponent& c,
1976  const Vector3D &d)
1977 {
1978  double matr[9], f;
1979  double adj_m[9], g;
1980  double dg[9], loc0, loc1, loc2, loc3, loc4;
1981  double A[3], J_A[6], J_B[9], J_C[9], cross;
1982 
1983  const double scale[6] = {
1984  d[0]*d[0], d[0]*d[1], d[0]*d[2],
1985  d[1]*d[1], d[1]*d[2],
1986  d[2]*d[2]
1987  };
1988 
1989  /* Calculate M = A*inv(W). */
1990  matr[0] = d[0]*(x[1][0] - x[0][0]);
1991  matr[1] = d[1]*(x[2][0] - x[0][0]);
1992  matr[2] = d[2]*(x[3][0] - x[0][0]);
1993 
1994  matr[3] = d[0]*(x[1][1] - x[0][1]);
1995  matr[4] = d[1]*(x[2][1] - x[0][1]);
1996  matr[5] = d[2]*(x[3][1] - x[0][1]);
1997 
1998  matr[6] = d[0]*(x[1][2] - x[0][2]);
1999  matr[7] = d[1]*(x[2][2] - x[0][2]);
2000  matr[8] = d[2]*(x[3][2] - x[0][2]);
2001 
2002  /* Calculate det(M). */
2003  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
2004  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
2005  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
2006  g = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
2007  if (g < MSQ_MIN) { obj = g; return false; }
2008 
2009  /* Calculate norm(M). */
2010  f = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] +
2011  matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] +
2012  matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8];
2013 
2014  loc3 = f;
2015  loc4 = g;
2016 
2017  /* Calculate objective function. */
2018  obj = a * pow(f, b) * pow(g, c);
2019 
2020  /* Calculate the derivative of the objective function. */
2021  f = b * obj / f; /* Constant on nabla f */
2022  g = c * obj / g; /* Constant on nable g */
2023  f *= 2.0; /* Modification for nabla f */
2024 
2025  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
2026  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
2027  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
2028  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
2029  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
2030  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
2031 
2032  adj_m[0] = d[0]*(matr[0]*f + dg[0]*g);
2033  adj_m[1] = d[1]*(matr[1]*f + dg[1]*g);
2034  adj_m[2] = d[2]*(matr[2]*f + dg[2]*g);
2035  adj_m[3] = d[0]*(matr[3]*f + dg[3]*g);
2036  adj_m[4] = d[1]*(matr[4]*f + dg[4]*g);
2037  adj_m[5] = d[2]*(matr[5]*f + dg[5]*g);
2038  adj_m[6] = d[0]*(matr[6]*f + dg[6]*g);
2039  adj_m[7] = d[1]*(matr[7]*f + dg[7]*g);
2040  adj_m[8] = d[2]*(matr[8]*f + dg[8]*g);
2041 
2042  g_obj[0][0] = -adj_m[0] - adj_m[1] - adj_m[2];
2043  g_obj[1][0] = adj_m[0];
2044  g_obj[2][0] = adj_m[1];
2045  g_obj[3][0] = adj_m[2];
2046 
2047  g_obj[0][1] = -adj_m[3] - adj_m[4] - adj_m[5];
2048  g_obj[1][1] = adj_m[3];
2049  g_obj[2][1] = adj_m[4];
2050  g_obj[3][1] = adj_m[5];
2051 
2052  g_obj[0][2] = -adj_m[6] - adj_m[7] - adj_m[8];
2053  g_obj[1][2] = adj_m[6];
2054  g_obj[2][2] = adj_m[7];
2055  g_obj[3][2] = adj_m[8];
2056 
2057  /* Calculate the hessian of the objective. */
2058  loc0 = f; /* Constant on nabla^2 f */
2059  loc1 = g; /* Constant on nabla^2 g */
2060  cross = f * c / loc4; /* Constant on nabla g nabla f */
2061  f = f * (b-1) / loc3; /* Constant on nabla f nabla f */
2062  g = g * (c-1) / loc4; /* Constant on nabla g nabla g */
2063  f *= 2.0; /* Modification for nabla f */
2064 
2065  /* First block of rows */
2066  loc3 = matr[0]*f + dg[0]*cross;
2067  loc4 = dg[0]*g + matr[0]*cross;
2068 
2069  J_A[0] = loc0 + loc3*matr[0] + loc4*dg[0];
2070  J_A[1] = loc3*matr[1] + loc4*dg[1];
2071  J_A[2] = loc3*matr[2] + loc4*dg[2];
2072  J_B[0] = loc3*matr[3] + loc4*dg[3];
2073  J_B[1] = loc3*matr[4] + loc4*dg[4];
2074  J_B[2] = loc3*matr[5] + loc4*dg[5];
2075  J_C[0] = loc3*matr[6] + loc4*dg[6];
2076  J_C[1] = loc3*matr[7] + loc4*dg[7];
2077  J_C[2] = loc3*matr[8] + loc4*dg[8];
2078 
2079  loc3 = matr[1]*f + dg[1]*cross;
2080  loc4 = dg[1]*g + matr[1]*cross;
2081 
2082  J_A[3] = loc0 + loc3*matr[1] + loc4*dg[1];
2083  J_A[4] = loc3*matr[2] + loc4*dg[2];
2084  J_B[3] = loc3*matr[3] + loc4*dg[3];
2085  J_B[4] = loc3*matr[4] + loc4*dg[4];
2086  J_B[5] = loc3*matr[5] + loc4*dg[5];
2087  J_C[3] = loc3*matr[6] + loc4*dg[6];
2088  J_C[4] = loc3*matr[7] + loc4*dg[7];
2089  J_C[5] = loc3*matr[8] + loc4*dg[8];
2090 
2091  loc3 = matr[2]*f + dg[2]*cross;
2092  loc4 = dg[2]*g + matr[2]*cross;
2093 
2094  J_A[5] = loc0 + loc3*matr[2] + loc4*dg[2];
2095  J_B[6] = loc3*matr[3] + loc4*dg[3];
2096  J_B[7] = loc3*matr[4] + loc4*dg[4];
2097  J_B[8] = loc3*matr[5] + loc4*dg[5];
2098  J_C[6] = loc3*matr[6] + loc4*dg[6];
2099  J_C[7] = loc3*matr[7] + loc4*dg[7];
2100  J_C[8] = loc3*matr[8] + loc4*dg[8];
2101 
2102  /* First diagonal block */
2103  J_A[0] *= scale[0];
2104  J_A[1] *= scale[1];
2105  J_A[2] *= scale[2];
2106  J_A[3] *= scale[3];
2107  J_A[4] *= scale[4];
2108  J_A[5] *= scale[5];
2109 
2110  A[0] = -J_A[0] - J_A[1] - J_A[2];
2111  A[1] = -J_A[1] - J_A[3] - J_A[4];
2112  A[2] = -J_A[2] - J_A[4] - J_A[5];
2113 
2114  h_obj[0][0][0] = -A[0] - A[1] - A[2];
2115  h_obj[1][0][0] = A[0];
2116  h_obj[2][0][0] = A[1];
2117  h_obj[3][0][0] = A[2];
2118 
2119  h_obj[4][0][0] = J_A[0];
2120  h_obj[5][0][0] = J_A[1];
2121  h_obj[6][0][0] = J_A[2];
2122 
2123  h_obj[7][0][0] = J_A[3];
2124  h_obj[8][0][0] = J_A[4];
2125 
2126  h_obj[9][0][0] = J_A[5];
2127 
2128  /* First off-diagonal block */
2129  loc2 = matr[8]*loc1;
2130  J_B[1] += loc2;
2131  J_B[3] -= loc2;
2132 
2133  loc2 = matr[7]*loc1;
2134  J_B[2] -= loc2;
2135  J_B[6] += loc2;
2136 
2137  loc2 = matr[6]*loc1;
2138  J_B[5] += loc2;
2139  J_B[7] -= loc2;
2140 
2141  J_B[0] *= scale[0];
2142  J_B[1] *= scale[1];
2143  J_B[2] *= scale[2];
2144  J_B[3] *= scale[1];
2145  J_B[4] *= scale[3];
2146  J_B[5] *= scale[4];
2147  J_B[6] *= scale[2];
2148  J_B[7] *= scale[4];
2149  J_B[8] *= scale[5];
2150 
2151  A[0] = -J_B[0] - J_B[3] - J_B[6];
2152  A[1] = -J_B[1] - J_B[4] - J_B[7];
2153  A[2] = -J_B[2] - J_B[5] - J_B[8];
2154 
2155  h_obj[0][0][1] = -A[0] - A[1] - A[2];
2156  h_obj[1][0][1] = A[0];
2157  h_obj[2][0][1] = A[1];
2158  h_obj[3][0][1] = A[2];
2159 
2160  h_obj[1][1][0] = -J_B[0] - J_B[1] - J_B[2];
2161  h_obj[4][0][1] = J_B[0];
2162  h_obj[5][0][1] = J_B[1];
2163  h_obj[6][0][1] = J_B[2];
2164 
2165  h_obj[2][1][0] = -J_B[3] - J_B[4] - J_B[5];
2166  h_obj[5][1][0] = J_B[3];
2167  h_obj[7][0][1] = J_B[4];
2168  h_obj[8][0][1] = J_B[5];
2169 
2170  h_obj[3][1][0] = -J_B[6] - J_B[7] - J_B[8];
2171  h_obj[6][1][0] = J_B[6];
2172  h_obj[8][1][0] = J_B[7];
2173  h_obj[9][0][1] = J_B[8];
2174 
2175  /* Second off-diagonal block */
2176  loc2 = matr[5]*loc1;
2177  J_C[1] -= loc2;
2178  J_C[3] += loc2;
2179 
2180  loc2 = matr[4]*loc1;
2181  J_C[2] += loc2;
2182  J_C[6] -= loc2;
2183 
2184  loc2 = matr[3]*loc1;
2185  J_C[5] -= loc2;
2186  J_C[7] += loc2;
2187 
2188  J_C[0] *= scale[0];
2189  J_C[1] *= scale[1];
2190  J_C[2] *= scale[2];
2191  J_C[3] *= scale[1];
2192  J_C[4] *= scale[3];
2193  J_C[5] *= scale[4];
2194  J_C[6] *= scale[2];
2195  J_C[7] *= scale[4];
2196  J_C[8] *= scale[5];
2197 
2198  A[0] = -J_C[0] - J_C[3] - J_C[6];
2199  A[1] = -J_C[1] - J_C[4] - J_C[7];
2200  A[2] = -J_C[2] - J_C[5] - J_C[8];
2201 
2202  h_obj[0][0][2] = -A[0] - A[1] - A[2];
2203  h_obj[1][0][2] = A[0];
2204  h_obj[2][0][2] = A[1];
2205  h_obj[3][0][2] = A[2];
2206 
2207  h_obj[1][2][0] = -J_C[0] - J_C[1] - J_C[2];
2208  h_obj[4][0][2] = J_C[0];
2209  h_obj[5][0][2] = J_C[1];
2210  h_obj[6][0][2] = J_C[2];
2211 
2212  h_obj[2][2][0] = -J_C[3] - J_C[4] - J_C[5];
2213  h_obj[5][2][0] = J_C[3];
2214  h_obj[7][0][2] = J_C[4];
2215  h_obj[8][0][2] = J_C[5];
2216 
2217  h_obj[3][2][0] = -J_C[6] - J_C[7] - J_C[8];
2218  h_obj[6][2][0] = J_C[6];
2219  h_obj[8][2][0] = J_C[7];
2220  h_obj[9][0][2] = J_C[8];
2221 
2222  /* Second block of rows */
2223  loc3 = matr[3]*f + dg[3]*cross;
2224  loc4 = dg[3]*g + matr[3]*cross;
2225 
2226  J_A[0] = loc0 + loc3*matr[3] + loc4*dg[3];
2227  J_A[1] = loc3*matr[4] + loc4*dg[4];
2228  J_A[2] = loc3*matr[5] + loc4*dg[5];
2229  J_B[0] = loc3*matr[6] + loc4*dg[6];
2230  J_B[1] = loc3*matr[7] + loc4*dg[7];
2231  J_B[2] = loc3*matr[8] + loc4*dg[8];
2232 
2233  loc3 = matr[4]*f + dg[4]*cross;
2234  loc4 = dg[4]*g + matr[4]*cross;
2235 
2236  J_A[3] = loc0 + loc3*matr[4] + loc4*dg[4];
2237  J_A[4] = loc3*matr[5] + loc4*dg[5];
2238  J_B[3] = loc3*matr[6] + loc4*dg[6];
2239  J_B[4] = loc3*matr[7] + loc4*dg[7];
2240  J_B[5] = loc3*matr[8] + loc4*dg[8];
2241 
2242  loc3 = matr[5]*f + dg[5]*cross;
2243  loc4 = dg[5]*g + matr[5]*cross;
2244 
2245  J_A[5] = loc0 + loc3*matr[5] + loc4*dg[5];
2246  J_B[6] = loc3*matr[6] + loc4*dg[6];
2247  J_B[7] = loc3*matr[7] + loc4*dg[7];
2248  J_B[8] = loc3*matr[8] + loc4*dg[8];
2249 
2250  /* Second diagonal block */
2251  J_A[0] *= scale[0];
2252  J_A[1] *= scale[1];
2253  J_A[2] *= scale[2];
2254  J_A[3] *= scale[3];
2255  J_A[4] *= scale[4];
2256  J_A[5] *= scale[5];
2257 
2258  A[0] = -J_A[0] - J_A[1] - J_A[2];
2259  A[1] = -J_A[1] - J_A[3] - J_A[4];
2260  A[2] = -J_A[2] - J_A[4] - J_A[5];
2261 
2262  h_obj[0][1][1] = -A[0] - A[1] - A[2];
2263  h_obj[1][1][1] = A[0];
2264  h_obj[2][1][1] = A[1];
2265  h_obj[3][1][1] = A[2];
2266 
2267  h_obj[4][1][1] = J_A[0];
2268  h_obj[5][1][1] = J_A[1];
2269  h_obj[6][1][1] = J_A[2];
2270 
2271  h_obj[7][1][1] = J_A[3];
2272  h_obj[8][1][1] = J_A[4];
2273 
2274  h_obj[9][1][1] = J_A[5];
2275 
2276  /* Third off-diagonal block */
2277  loc2 = matr[2]*loc1;
2278  J_B[1] += loc2;
2279  J_B[3] -= loc2;
2280 
2281  loc2 = matr[1]*loc1;
2282  J_B[2] -= loc2;
2283  J_B[6] += loc2;
2284 
2285  loc2 = matr[0]*loc1;
2286  J_B[5] += loc2;
2287  J_B[7] -= loc2;
2288 
2289  J_B[0] *= scale[0];
2290  J_B[1] *= scale[1];
2291  J_B[2] *= scale[2];
2292  J_B[3] *= scale[1];
2293  J_B[4] *= scale[3];
2294  J_B[5] *= scale[4];
2295  J_B[6] *= scale[2];
2296  J_B[7] *= scale[4];
2297  J_B[8] *= scale[5];
2298 
2299  A[0] = -J_B[0] - J_B[3] - J_B[6];
2300  A[1] = -J_B[1] - J_B[4] - J_B[7];
2301  A[2] = -J_B[2] - J_B[5] - J_B[8];
2302 
2303  h_obj[0][1][2] = -A[0] - A[1] - A[2];
2304  h_obj[1][1][2] = A[0];
2305  h_obj[2][1][2] = A[1];
2306  h_obj[3][1][2] = A[2];
2307 
2308  h_obj[1][2][1] = -J_B[0] - J_B[1] - J_B[2];
2309  h_obj[4][1][2] = J_B[0];
2310  h_obj[5][1][2] = J_B[1];
2311  h_obj[6][1][2] = J_B[2];
2312 
2313  h_obj[2][2][1] = -J_B[3] - J_B[4] - J_B[5];
2314  h_obj[5][2][1] = J_B[3];
2315  h_obj[7][1][2] = J_B[4];
2316  h_obj[8][1][2] = J_B[5];
2317 
2318  h_obj[3][2][1] = -J_B[6] - J_B[7] - J_B[8];
2319  h_obj[6][2][1] = J_B[6];
2320  h_obj[8][2][1] = J_B[7];
2321  h_obj[9][1][2] = J_B[8];
2322 
2323  /* Third block of rows */
2324  loc3 = matr[6]*f + dg[6]*cross;
2325  loc4 = dg[6]*g + matr[6]*cross;
2326 
2327  J_A[0] = loc0 + loc3*matr[6] + loc4*dg[6];
2328  J_A[1] = loc3*matr[7] + loc4*dg[7];
2329  J_A[2] = loc3*matr[8] + loc4*dg[8];
2330 
2331  loc3 = matr[7]*f + dg[7]*cross;
2332  loc4 = dg[7]*g + matr[7]*cross;
2333 
2334  J_A[3] = loc0 + loc3*matr[7] + loc4*dg[7];
2335  J_A[4] = loc3*matr[8] + loc4*dg[8];
2336 
2337  loc3 = matr[8]*f + dg[8]*cross;
2338  loc4 = dg[8]*g + matr[8]*cross;
2339 
2340  J_A[5] = loc0 + loc3*matr[8] + loc4*dg[8];
2341 
2342  /* Third diagonal block */
2343  J_A[0] *= scale[0];
2344  J_A[1] *= scale[1];
2345  J_A[2] *= scale[2];
2346  J_A[3] *= scale[3];
2347  J_A[4] *= scale[4];
2348  J_A[5] *= scale[5];
2349 
2350  A[0] = -J_A[0] - J_A[1] - J_A[2];
2351  A[1] = -J_A[1] - J_A[3] - J_A[4];
2352  A[2] = -J_A[2] - J_A[4] - J_A[5];
2353 
2354  h_obj[0][2][2] = -A[0] - A[1] - A[2];
2355  h_obj[1][2][2] = A[0];
2356  h_obj[2][2][2] = A[1];
2357  h_obj[3][2][2] = A[2];
2358 
2359  h_obj[4][2][2] = J_A[0];
2360  h_obj[5][2][2] = J_A[1];
2361  h_obj[6][2][2] = J_A[2];
2362 
2363  h_obj[7][2][2] = J_A[3];
2364  h_obj[8][2][2] = J_A[4];
2365 
2366  h_obj[9][2][2] = J_A[5];
2367 
2368  // completes diagonal blocks.
2369  h_obj[0].fill_lower_triangle();
2370  h_obj[4].fill_lower_triangle();
2371  h_obj[7].fill_lower_triangle();
2372  h_obj[9].fill_lower_triangle();
2373  return true;
2374 }
2375 
2376 }
2377 
2378 #endif
Mesquite::h_fcn_3i
int h_fcn_3i(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const double a, const Exponent &b, const Exponent &c, const Vector3D &d)
Definition: includeLinks/MeanRatioFunctions.hpp:1973
Mesquite::g_fcn_2i
bool g_fcn_2i(double &obj, Vector3D g_obj[3], const Vector3D x[3], const Vector3D &n, const double a, const Exponent &b, const Exponent &c, const Vector3D &d)
Definition: includeLinks/MeanRatioFunctions.hpp:584
Mesquite::m_fcn_3e
bool m_fcn_3e(double &obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:931
Mesquite::g_fcn_2e
bool g_fcn_2e(double &obj, Vector3D g_obj[3], const Vector3D x[3], const Vector3D &n, const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:170
nvc::cross
NVec< 3, T > cross(const NVec< 3, T > &u, const NVec< 3, T > &v)
Definition: Rocon/include/NVec.h:185
Mesquite::g_fcn_3e
bool g_fcn_3e(double &obj, Vector3D g_obj[4], const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:973
d
const NT & d
Definition: rational_rotation.h:73
Mesquite::m_fcn_3i
bool m_fcn_3i(double &obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c, const Vector3D &d)
Definition: includeLinks/MeanRatioFunctions.hpp:1855
Mesquite::h_fcn_3e_v2
bool h_fcn_3e_v2(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1831
Mesquite::Vector3D
Vector3D is the object that effeciently stores information about about three-deminsional vectors...
Definition: includeLinks/Vector3D.hpp:64
Mesquite::m_fcn_2i
bool m_fcn_2i(double &obj, const Vector3D x[3], const Vector3D &n, const double a, const Exponent &b, const Exponent &c, const Vector3D &d)
Definition: includeLinks/MeanRatioFunctions.hpp:543
Mesquite::g_fcn_3i
bool g_fcn_3i(double &obj, Vector3D g_obj[4], const Vector3D x[4], const double a, const Exponent &b, const Exponent &c, const Vector3D &d)
Definition: includeLinks/MeanRatioFunctions.hpp:1895
Mesquite::h_fcn_3e_v3
bool h_fcn_3e_v3(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1717
Mesquite::g_fcn_3e_v0
bool g_fcn_3e_v0(double &obj, Vector3D &g_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1681
A
rational * A
Definition: vinci_lass.c:67
Mesquite::h_fcn_3e
bool h_fcn_3e(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1062
Mesquite::Matrix3D
3*3 Matric class, row-oriented, 0-based [i][j] indexing.
Definition: includeLinks/Matrix3D.hpp:78
Mesquite::m_fcn_2e
bool m_fcn_2e(double &obj, const Vector3D x[3], const Vector3D &n, const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:131
Mesquite::g_fcn_3e_v1
bool g_fcn_3e_v1(double &obj, Vector3D &g_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1693
Mesquite::h_fcn_3e_v0
bool h_fcn_3e_v0(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1805
x
void int int REAL * x
Definition: read.cpp:74
n
const NT & n
Definition: rational_rotation.h:72
Mesquite::h_fcn_2e
bool h_fcn_2e(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6], const Vector3D x[3], const Vector3D &n, const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:244
Mesquite::h_fcn_3e_v1
bool h_fcn_3e_v1(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1818
Mesquite::h_fcn_2i
bool h_fcn_2i(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6], const Vector3D x[3], const Vector3D &n, const double a, const Exponent &b, const Exponent &c, const Vector3D &d)
Definition: includeLinks/MeanRatioFunctions.hpp:656
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89
Mesquite::g_fcn_3e_v3
bool g_fcn_3e_v3(double &obj, Vector3D &g_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1629
Mesquite::g_fcn_3e_v2
bool g_fcn_3e_v2(double &obj, Vector3D &g_obj, const Vector3D x[4], const double a, const Exponent &b, const Exponent &c)
Definition: includeLinks/MeanRatioFunctions.hpp:1705
Mesquite::MSQ_MIN
const double MSQ_MIN
Definition: Mesquite.hpp:160