Rocstar  1.0
Rocstar multiphysics simulation application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
includeLinks/I_DFTFamilyFunctions.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 
82 #ifndef I_DFTFamilyFunctions_hpp
83 #define I_DFTFamilyFunctions_hpp
84 
85 #include <math.h>
86 #include "Mesquite.hpp"
87 #include "Vector3D.hpp"
88 #include "Matrix3D.hpp"
89 
90 namespace Mesquite
91 {
92  //NOTE (mbrewer): variables in the following discription may not be
93  //consistent with the variables used above.
94  /***************************************************************************/
95  /* Gradient calculation courtesy of Paul Hovland. The code was modified */
96  /* to reduce the number of flops and intermediate variables, and improve */
97  /* the locality of reference. */
98  /***************************************************************************/
99  /* The Hessian calculation is computes everyting in (x,y,z) blocks and */
100  /* stores only the upper triangular blocks. The results are put into */
101  /* (c1,c2,c3,c4) order prior to returning the Hessian matrix. */
102  /***************************************************************************/
103  /* The form of the function, gradient, and Hessian follows: */
104  /* o(x) = a * pow(f(A(x)), b) * pow(g(A(x)), c) */
105  /* where A(x) is the incidence matrix generated by: */
106  /* [x1-x0 x2-x0 x3-x0] */
107  /* A(x) = [y1-y0 y2-y0 y3-y0] * inv(W) */
108  /* [z1-z0 z2-z0 z3-z0] */
109  /* and f() is the squared Frobenius norm of A(x), and g() is the */
110  /* determinant of A(x). */
111  /* */
112  /* The gradient is calculated as follows: */
113  /* gamma := a*b*pow(f(A(x)),b-1)*pow(g(A(x)),c) */
114  /* delta := a*c*pow(f(A(x)),b)*pow(g(A(x)),c-1) */
115  /* */
116  /* do/dx = (gamma * (df/dA) + delta * (dg/dA)) (dA/dx) */
117  /* */
118  /* (df/dA)_i = 2*A_i */
119  /* (dg/dA)_i = A_j*A_k - A_l*A_m for some {j,k,l,m} */
120  /* */
121  /* d^2o/dx^2 = (dA/dx)' * ((d gamma/dA) * (df/dA) + */
122  /* (d delta/dA) * (dg/dA) */
123  /* gamma * (d^2f/dA^2) */
124  /* delta * (d^2g/dA^2)) * (dA/dx) */
125  /* */
126  /* Note: since A(x) is a linear function, there are no terms involving */
127  /* d^2A/dx^2 since this matrix is zero. */
128  /* */
129  /* gamma := a*b*c*pow(f(A(x)),b-1)*pow(g(A(x)),c-1) */
130  /* beta := a*c*(c-1)*pow(f(A(x)),b)*pow(g(A(x)),c-2) */
131  /* psi := a*b*(b-1)*pow(f(A(x)),b-2)*pow(g(A(x)),c) */
132  /* */
133  /* d^2o/dx^2 = (dA/dx)' * (gamma*((dg/dA)'*(df/dA) + (df/dA)'*(dg/dA)) + */
134  /* beta* (dg/dA)'*(dg/dA) + */
135  /* psi* (df/dA)'*(df/dA) + */
136  /* gamma*(d^2f/dA^2) + */
137  /* delta*(d^2g/dA^2)) * (dA/dx) */
138  /* */
139  /* Note: (df/dA) and (dg/dA) are row vectors and we only calculate the */
140  /* upper triangular part of the inner matrices. */
141  /***************************************************************************/
142 
143  /***************************************************************************/
144  /* Triangular and quadrilateral versions of the metric. */
145  /***************************************************************************/
146 
147  inline bool m_gdft_2(double &obj,
148  const Vector3D x[3], const Vector3D &n,
149  const Matrix3D &invR, /* upper triangular */
150  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
151  const double alpha = 0.5,/* constant */
152  const Exponent& gamma = 1.0,/* planar elements */
153  const double delta = 0.0,/* max in denominator */
154  const double beta = 0.0)/* no modification */
155  {
156  double matr[9], f, t1, t2;
157  double matd[9], g;
158 
159  /* Calculate M = A*inv(R). */
160  f = x[1][0] - x[0][0];
161  g = x[2][0] - x[0][0];
162  matr[0] = f*invR[0][0];
163  matr[1] = f*invR[0][1] + g*invR[1][1];
164  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
165 
166  f = x[1][1] - x[0][1];
167  g = x[2][1] - x[0][1];
168  matr[3] = f*invR[0][0];
169  matr[4] = f*invR[0][1] + g*invR[1][1];
170  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
171 
172  f = x[1][2] - x[0][2];
173  g = x[2][2] - x[0][2];
174  matr[6] = f*invR[0][0];
175  matr[7] = f*invR[0][1] + g*invR[1][1];
176  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
177 
178  /* Calculate det(M). */
179  t1 = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
180  matr[3]*(matr[2]*matr[7] - matr[1]*matr[8]) +
181  matr[6]*(matr[1]*matr[5] - matr[2]*matr[4]);
182 
183  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
184 
185  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
186  t2 = sqrt(t1*t1 + 4.0*delta*delta);
187  g = t1 + t2;
188 
189  /* Calculate N = M - beta*Q. */
190  matd[0] = matr[0] - beta*Q[0][0];
191  matd[1] = matr[1] - beta*Q[0][1];
192  matd[2] = matr[2] - beta*Q[0][2];
193  matd[3] = matr[3] - beta*Q[1][0];
194  matd[4] = matr[4] - beta*Q[1][1];
195  matd[5] = matr[5] - beta*Q[1][2];
196  matd[6] = matr[6] - beta*Q[2][0];
197  matd[7] = matr[7] - beta*Q[2][1];
198  matd[8] = matr[8] - beta*Q[2][2];
199 
200  /* Calculate norm(N) */
201  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
202  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
203  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
204 
205  /* Calculate objective function. */
206  obj = alpha * pow(2.0, gamma) * f / pow(g, gamma);
207  return true;
208  }
209 
210  inline bool g_gdft_2(double &obj, Vector3D g_obj[3],
211  const Vector3D x[3], const Vector3D &n,
212  const Matrix3D &invR, /* upper triangular */
213  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
214  const double alpha = 0.5,/* constant */
215  const Exponent& gamma = 1.0,/* simplicial elements */
216  const double delta = 0.0,/* max in denominator */
217  const double beta = 0.0)/* no modification */
218  {
219  double matr[9], f, t1, t2;
220  double matd[9], g;
221  double adjm[9], loc1, loc2, loc3, loc4;
222 
223  /* Calculate M = A*inv(R). */
224  f = x[1][0] - x[0][0];
225  g = x[2][0] - x[0][0];
226  matr[0] = f*invR[0][0];
227  matr[1] = f*invR[0][1] + g*invR[1][1];
228  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
229 
230  f = x[1][1] - x[0][1];
231  g = x[2][1] - x[0][1];
232  matr[3] = f*invR[0][0];
233  matr[4] = f*invR[0][1] + g*invR[1][1];
234  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
235 
236  f = x[1][2] - x[0][2];
237  g = x[2][2] - x[0][2];
238  matr[6] = f*invR[0][0];
239  matr[7] = f*invR[0][1] + g*invR[1][1];
240  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
241 
242  /* Calculate det(M). */
243  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
244  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
245  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
246  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
247 
248  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
249 
250  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
251  t2 = sqrt(t1*t1 + 4.0*delta*delta);
252  g = t1 + t2;
253 
254  /* Calculate N = M - beta*Q. */
255  matd[0] = matr[0] - beta*Q[0][0];
256  matd[1] = matr[1] - beta*Q[0][1];
257  matd[2] = matr[2] - beta*Q[0][2];
258  matd[3] = matr[3] - beta*Q[1][0];
259  matd[4] = matr[4] - beta*Q[1][1];
260  matd[5] = matr[5] - beta*Q[1][2];
261  matd[6] = matr[6] - beta*Q[2][0];
262  matd[7] = matr[7] - beta*Q[2][1];
263  matd[8] = matr[8] - beta*Q[2][2];
264 
265  /* Calculate norm(N) */
266  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
267  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
268  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
269 
270  /* Calculate objective function. */
271  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
272  obj = f * loc4;
273 
274  /* Calculate the derivative of the objective function. */
275  f = 2.0 * loc4;
276  g = -gamma * obj / t2;
277 
278  /* Calculate adjoint matrix */
279  adjm[0] = f*matd[0] + g*loc1;
280  adjm[1] = f*matd[1] + g*loc2;
281  adjm[2] = f*matd[2] + g*loc3;
282 
283  loc1 = g*matr[0];
284  loc2 = g*matr[1];
285  loc3 = g*matr[2];
286 
287  adjm[3] = f*matd[3] + loc3*matr[7] - loc2*matr[8];
288  adjm[4] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
289  adjm[5] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
290 
291  adjm[6] = f*matd[6] + loc2*matr[5] - loc3*matr[4];
292  adjm[7] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
293  adjm[8] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
294 
295  /* Construct gradients */
296  g_obj[1][0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
297  g_obj[2][0] = invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
298  g_obj[0][0] = -g_obj[1][0] - g_obj[2][0];
299 
300  g_obj[1][1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
301  g_obj[2][1] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
302  g_obj[0][1] = -g_obj[1][1] - g_obj[2][1];
303 
304  g_obj[1][2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
305  g_obj[2][2] = invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
306  g_obj[0][2] = -g_obj[1][2] - g_obj[2][2];
307  return true;
308  }
309 
310  inline bool h_gdft_2(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6],
311  const Vector3D x[3], const Vector3D &n,
312  const Matrix3D &invR, /* upper triangular */
313  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
314  const double alpha = 0.5,/* constant */
315  const Exponent& gamma = 1.0,/* simplicial elements */
316  const double delta = 0.0,/* max in denominator */
317  const double beta = 0.0)/* no modification */
318  {
319  double matr[9], f, t1, t2;
320  double matd[9], g, t3, loc1;
321  double adjm[9], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
322  double J_A[6], J_B[10], J_C[10], J_D[6], J_E[10], J_F[6];
323  double A[9];
324 
325  /* Calculate M = A*inv(R). */
326  f = x[1][0] - x[0][0];
327  g = x[2][0] - x[0][0];
328  matr[0] = f*invR[0][0];
329  matr[1] = f*invR[0][1] + g*invR[1][1];
330  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
331 
332  f = x[1][1] - x[0][1];
333  g = x[2][1] - x[0][1];
334  matr[3] = f*invR[0][0];
335  matr[4] = f*invR[0][1] + g*invR[1][1];
336  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
337 
338  f = x[1][2] - x[0][2];
339  g = x[2][2] - x[0][2];
340  matr[6] = f*invR[0][0];
341  matr[7] = f*invR[0][1] + g*invR[1][1];
342  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
343 
344  /* Calculate det(M). */
345  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
346  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
347  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
348  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
349  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
350  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
351  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
352  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
353  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
354 
355  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
356 
357  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
358 
359  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
360  t2 = t1*t1 + 4.0*delta*delta;
361  t3 = sqrt(t2);
362  g = t1 + t3;
363 
364  /* Calculate N = M - beta*Q. */
365  matd[0] = matr[0] - beta*Q[0][0];
366  matd[1] = matr[1] - beta*Q[0][1];
367  matd[2] = matr[2] - beta*Q[0][2];
368  matd[3] = matr[3] - beta*Q[1][0];
369  matd[4] = matr[4] - beta*Q[1][1];
370  matd[5] = matr[5] - beta*Q[1][2];
371  matd[6] = matr[6] - beta*Q[2][0];
372  matd[7] = matr[7] - beta*Q[2][1];
373  matd[8] = matr[8] - beta*Q[2][2];
374 
375  /* Calculate norm(N) */
376  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
377  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
378  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
379 
380  /* Calculate objective function. */
381  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
382  obj = f * loc1;
383 
384  /* Calculate the derivative of the objective function. */
385  t3 = 1.0 / t3;
386  dobj_df = 2.0 * loc1;
387  dobj_dg = -gamma * obj * t3;
388  dobj_dfdg = -gamma * dobj_df * t3;
389  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
390 
391  /* Calculate adjoint matrix */
392  adjm[0] = dobj_df*matd[0] + dobj_dg*dg[0];
393  adjm[1] = dobj_df*matd[1] + dobj_dg*dg[1];
394  adjm[2] = dobj_df*matd[2] + dobj_dg*dg[2];
395  adjm[3] = dobj_df*matd[3] + dobj_dg*dg[3];
396  adjm[4] = dobj_df*matd[4] + dobj_dg*dg[4];
397  adjm[5] = dobj_df*matd[5] + dobj_dg*dg[5];
398  adjm[6] = dobj_df*matd[6] + dobj_dg*dg[6];
399  adjm[7] = dobj_df*matd[7] + dobj_dg*dg[7];
400  adjm[8] = dobj_df*matd[8] + dobj_dg*dg[8];
401 
402  /* Construct gradients */
403  g_obj[1][0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
404  g_obj[2][0] = invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
405  g_obj[0][0] = -g_obj[1][0] - g_obj[2][0];
406 
407  g_obj[1][1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
408  g_obj[2][1] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
409  g_obj[0][1] = -g_obj[1][1] - g_obj[2][1];
410 
411  g_obj[1][2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
412  g_obj[2][2] = invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
413  g_obj[0][2] = -g_obj[1][2] - g_obj[2][2];
414 
415  /* Start of the Hessian evaluation */
416  adjm[0] = dobj_dg*matr[0]; matd[0] *= dobj_dfdg;
417  adjm[1] = dobj_dg*matr[1]; matd[1] *= dobj_dfdg;
418  adjm[2] = dobj_dg*matr[2]; matd[2] *= dobj_dfdg;
419  adjm[3] = dobj_dg*matr[3]; matd[3] *= dobj_dfdg;
420  adjm[4] = dobj_dg*matr[4]; matd[4] *= dobj_dfdg;
421  adjm[5] = dobj_dg*matr[5]; matd[5] *= dobj_dfdg;
422  adjm[6] = dobj_dg*matr[6]; matd[6] *= dobj_dfdg;
423  adjm[7] = dobj_dg*matr[7]; matd[7] *= dobj_dfdg;
424  adjm[8] = dobj_dg*matr[8]; matd[8] *= dobj_dfdg;
425 
426  /* Blocks for the Hessian construction */
427  loc1 = dobj_dgdg*dg[0] + matd[0];
428  J_A[0] = dobj_df + dg[0]*(matd[0] + loc1);
429  J_A[1] = dg[0]*matd[1] + loc1*dg[1];
430  J_A[2] = dg[0]*matd[2] + loc1*dg[2];
431  J_B[0] = dg[0]*matd[3] + loc1*dg[3];
432  J_B[1] = dg[0]*matd[4] + loc1*dg[4] + adjm[8];
433  J_B[2] = dg[0]*matd[5] + loc1*dg[5] - adjm[7];
434  J_C[0] = dg[0]*matd[6] + loc1*dg[6];
435  J_C[1] = dg[0]*matd[7] + loc1*dg[7] - adjm[5];
436  J_C[2] = dg[0]*matd[8] + loc1*dg[8] + adjm[4];
437 
438  loc1 = dobj_dgdg*dg[1] + matd[1];
439  J_A[3] = dobj_df + dg[1]*(matd[1] + loc1);
440  J_A[4] = dg[1]*matd[2] + loc1*dg[2];
441  J_B[3] = dg[1]*matd[3] + loc1*dg[3] - adjm[8];
442  J_B[4] = dg[1]*matd[4] + loc1*dg[4];
443  J_B[5] = dg[1]*matd[5] + loc1*dg[5] + adjm[6];
444  J_C[3] = dg[1]*matd[6] + loc1*dg[6] + adjm[5];
445  J_C[4] = dg[1]*matd[7] + loc1*dg[7];
446  J_C[5] = dg[1]*matd[8] + loc1*dg[8] - adjm[3];
447 
448  loc1 = dobj_dgdg*dg[2] + matd[2];
449  J_A[5] = dobj_df + dg[2]*(matd[2] + loc1);
450  J_B[6] = dg[2]*matd[3] + loc1*dg[3] + adjm[7];
451  J_B[7] = dg[2]*matd[4] + loc1*dg[4] - adjm[6];
452  J_B[8] = dg[2]*matd[5] + loc1*dg[5];
453  J_C[6] = dg[2]*matd[6] + loc1*dg[6] - adjm[4];
454  J_C[7] = dg[2]*matd[7] + loc1*dg[7] + adjm[3];
455  J_C[8] = dg[2]*matd[8] + loc1*dg[8];
456 
457  loc1 = dobj_dgdg*dg[3] + matd[3];
458  J_D[0] = dobj_df + dg[3]*(matd[3] + loc1);
459  J_D[1] = dg[3]*matd[4] + loc1*dg[4];
460  J_D[2] = dg[3]*matd[5] + loc1*dg[5];
461  J_E[0] = dg[3]*matd[6] + loc1*dg[6];
462  J_E[1] = dg[3]*matd[7] + loc1*dg[7] + adjm[2];
463  J_E[2] = dg[3]*matd[8] + loc1*dg[8] - adjm[1];
464 
465  loc1 = dobj_dgdg*dg[4] + matd[4];
466  J_D[3] = dobj_df + dg[4]*(matd[4] + loc1);
467  J_D[4] = dg[4]*matd[5] + loc1*dg[5];
468  J_E[3] = dg[4]*matd[6] + loc1*dg[6] - adjm[2];
469  J_E[4] = dg[4]*matd[7] + loc1*dg[7];
470  J_E[5] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
471 
472  loc1 = dobj_dgdg*dg[5] + matd[5];
473  J_D[5] = dobj_df + dg[5]*(matd[5] + loc1);
474  J_E[6] = dg[5]*matd[6] + loc1*dg[6] + adjm[1];
475  J_E[7] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
476  J_E[8] = dg[5]*matd[8] + loc1*dg[8];
477 
478  loc1 = dobj_dgdg*dg[6] + matd[6];
479  J_F[0] = dobj_df + dg[6]*(matd[6] + loc1);
480  J_F[1] = dg[6]*matd[7] + loc1*dg[7];
481  J_F[2] = dg[6]*matd[8] + loc1*dg[8];
482 
483  loc1 = dobj_dgdg*dg[7] + matd[7];
484  J_F[3] = dobj_df + dg[7]*(matd[7] + loc1);
485  J_F[4] = dg[7]*matd[8] + loc1*dg[8];
486 
487  J_F[5] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
488 
489  /* Assemble matrix products */
490 
491  /* dx / dx */
492  A[1] = J_A[0]*invR[0][0] + J_A[1]*invR[0][1] + J_A[2]*invR[0][2];
493  A[2] = J_A[1]*invR[1][1] + J_A[2]*invR[1][2];
494  A[0] = -A[1] - A[2];
495 
496  A[4] = J_A[1]*invR[0][0] + J_A[3]*invR[0][1] + J_A[4]*invR[0][2];
497  A[5] = J_A[3]*invR[1][1] + J_A[4]*invR[1][2];
498  A[3] = -A[4] - A[5];
499 
500  A[7] = J_A[2]*invR[0][0] + J_A[4]*invR[0][1] + J_A[5]*invR[0][2];
501  A[8] = J_A[4]*invR[1][1] + J_A[5]*invR[1][2];
502  A[6] = -A[7] - A[8];
503 
504  h_obj[1][0][0] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
505  h_obj[2][0][0] = A[3]*invR[1][1] + A[6]*invR[1][2];
506  h_obj[0][0][0] = -h_obj[1][0][0] - h_obj[2][0][0];
507 
508  h_obj[3][0][0] = A[1]*invR[0][0] + A[4]*invR[0][1] + A[7]*invR[0][2];
509  h_obj[4][0][0] = A[4]*invR[1][1] + A[7]*invR[1][2];
510 
511  h_obj[5][0][0] = A[5]*invR[1][1] + A[8]*invR[1][2];
512 
513  /* dx / dy */
514  A[1] = J_B[0]*invR[0][0] + J_B[1]*invR[0][1] + J_B[2]*invR[0][2];
515  A[2] = J_B[1]*invR[1][1] + J_B[2]*invR[1][2];
516  A[0] = -A[1] - A[2];
517 
518  A[4] = J_B[3]*invR[0][0] + J_B[4]*invR[0][1] + J_B[5]*invR[0][2];
519  A[5] = J_B[4]*invR[1][1] + J_B[5]*invR[1][2];
520  A[3] = -A[4] - A[5];
521 
522  A[7] = J_B[6]*invR[0][0] + J_B[7]*invR[0][1] + J_B[8]*invR[0][2];
523  A[8] = J_B[7]*invR[1][1] + J_B[8]*invR[1][2];
524  A[6] = -A[7] - A[8];
525 
526  h_obj[1][1][0] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
527  h_obj[3][0][1] = A[1]*invR[0][0] + A[4]*invR[0][1] + A[7]*invR[0][2];
528  h_obj[4][0][1] = A[2]*invR[0][0] + A[5]*invR[0][1] + A[8]*invR[0][2];
529 
530  h_obj[2][1][0] = A[3]*invR[1][1] + A[6]*invR[1][2];
531  h_obj[4][1][0] = A[4]*invR[1][1] + A[7]*invR[1][2];
532  h_obj[5][0][1] = A[5]*invR[1][1] + A[8]*invR[1][2];
533 
534  h_obj[0][0][1] = -h_obj[1][1][0] - h_obj[2][1][0];
535  h_obj[1][0][1] = -h_obj[3][0][1] - h_obj[4][1][0];
536  h_obj[2][0][1] = -h_obj[4][0][1] - h_obj[5][0][1];
537 
538  /* dx / dz */
539  A[1] = J_C[0]*invR[0][0] + J_C[1]*invR[0][1] + J_C[2]*invR[0][2];
540  A[2] = J_C[1]*invR[1][1] + J_C[2]*invR[1][2];
541  A[0] = -A[1] - A[2];
542 
543  A[4] = J_C[3]*invR[0][0] + J_C[4]*invR[0][1] + J_C[5]*invR[0][2];
544  A[5] = J_C[4]*invR[1][1] + J_C[5]*invR[1][2];
545  A[3] = -A[4] - A[5];
546 
547  A[7] = J_C[6]*invR[0][0] + J_C[7]*invR[0][1] + J_C[8]*invR[0][2];
548  A[8] = J_C[7]*invR[1][1] + J_C[8]*invR[1][2];
549  A[6] = -A[7] - A[8];
550 
551  h_obj[1][2][0] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
552  h_obj[3][0][2] = A[1]*invR[0][0] + A[4]*invR[0][1] + A[7]*invR[0][2];
553  h_obj[4][0][2] = A[2]*invR[0][0] + A[5]*invR[0][1] + A[8]*invR[0][2];
554 
555  h_obj[2][2][0] = A[3]*invR[1][1] + A[6]*invR[1][2];
556  h_obj[4][2][0] = A[4]*invR[1][1] + A[7]*invR[1][2];
557  h_obj[5][0][2] = A[5]*invR[1][1] + A[8]*invR[1][2];
558 
559  h_obj[0][0][2] = -h_obj[1][2][0] - h_obj[2][2][0];
560  h_obj[1][0][2] = -h_obj[3][0][2] - h_obj[4][2][0];
561  h_obj[2][0][2] = -h_obj[4][0][2] - h_obj[5][0][2];
562 
563  /* dy / dy */
564  A[1] = J_D[0]*invR[0][0] + J_D[1]*invR[0][1] + J_D[2]*invR[0][2];
565  A[2] = J_D[1]*invR[1][1] + J_D[2]*invR[1][2];
566  A[0] = -A[1] - A[2];
567 
568  A[4] = J_D[1]*invR[0][0] + J_D[3]*invR[0][1] + J_D[4]*invR[0][2];
569  A[5] = J_D[3]*invR[1][1] + J_D[4]*invR[1][2];
570  A[3] = -A[4] - A[5];
571 
572  A[7] = J_D[2]*invR[0][0] + J_D[4]*invR[0][1] + J_D[5]*invR[0][2];
573  A[8] = J_D[4]*invR[1][1] + J_D[5]*invR[1][2];
574  A[6] = -A[7] - A[8];
575 
576  h_obj[1][1][1] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
577  h_obj[2][1][1] = A[3]*invR[1][1] + A[6]*invR[1][2];
578  h_obj[0][1][1] = -h_obj[1][1][1] - h_obj[2][1][1];
579 
580  h_obj[3][1][1] = A[1]*invR[0][0] + A[4]*invR[0][1] + A[7]*invR[0][2];
581  h_obj[4][1][1] = A[4]*invR[1][1] + A[7]*invR[1][2];
582 
583  h_obj[5][1][1] = A[5]*invR[1][1] + A[8]*invR[1][2];
584 
585  /* dy / dz */
586  A[1] = J_E[0]*invR[0][0] + J_E[1]*invR[0][1] + J_E[2]*invR[0][2];
587  A[2] = J_E[1]*invR[1][1] + J_E[2]*invR[1][2];
588  A[0] = -A[1] - A[2];
589 
590  A[4] = J_E[3]*invR[0][0] + J_E[4]*invR[0][1] + J_E[5]*invR[0][2];
591  A[5] = J_E[4]*invR[1][1] + J_E[5]*invR[1][2];
592  A[3] = -A[4] - A[5];
593 
594  A[7] = J_E[6]*invR[0][0] + J_E[7]*invR[0][1] + J_E[8]*invR[0][2];
595  A[8] = J_E[7]*invR[1][1] + J_E[8]*invR[1][2];
596  A[6] = -A[7] - A[8];
597 
598  h_obj[1][2][1] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
599  h_obj[3][1][2] = A[1]*invR[0][0] + A[4]*invR[0][1] + A[7]*invR[0][2];
600  h_obj[4][1][2] = A[2]*invR[0][0] + A[5]*invR[0][1] + A[8]*invR[0][2];
601 
602  h_obj[2][2][1] = A[3]*invR[1][1] + A[6]*invR[1][2];
603  h_obj[4][2][1] = A[4]*invR[1][1] + A[7]*invR[1][2];
604  h_obj[5][1][2] = A[5]*invR[1][1] + A[8]*invR[1][2];
605 
606  h_obj[0][1][2] = -h_obj[1][2][1] - h_obj[2][2][1];
607  h_obj[1][1][2] = -h_obj[3][1][2] - h_obj[4][2][1];
608  h_obj[2][1][2] = -h_obj[4][1][2] - h_obj[5][1][2];
609 
610  /* dz / dz */
611  A[1] = J_F[0]*invR[0][0] + J_F[1]*invR[0][1] + J_F[2]*invR[0][2];
612  A[2] = J_F[1]*invR[1][1] + J_F[2]*invR[1][2];
613  A[0] = -A[1] - A[2];
614 
615  A[4] = J_F[1]*invR[0][0] + J_F[3]*invR[0][1] + J_F[4]*invR[0][2];
616  A[5] = J_F[3]*invR[1][1] + J_F[4]*invR[1][2];
617  A[3] = -A[4] - A[5];
618 
619  A[7] = J_F[2]*invR[0][0] + J_F[4]*invR[0][1] + J_F[5]*invR[0][2];
620  A[8] = J_F[4]*invR[1][1] + J_F[5]*invR[1][2];
621  A[6] = -A[7] - A[8];
622 
623  h_obj[1][2][2] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
624  h_obj[2][2][2] = A[3]*invR[1][1] + A[6]*invR[1][2];
625  h_obj[0][2][2] = -h_obj[1][2][2] - h_obj[2][2][2];
626 
627  h_obj[3][2][2] = A[1]*invR[0][0] + A[4]*invR[0][1] + A[7]*invR[0][2];
628  h_obj[4][2][2] = A[4]*invR[1][1] + A[7]*invR[1][2];
629 
630  h_obj[5][2][2] = A[5]*invR[1][1] + A[8]*invR[1][2];
631 
632  /* Complete diagonal blocks */
633  h_obj[0].fill_lower_triangle();
634  h_obj[3].fill_lower_triangle();
635  h_obj[5].fill_lower_triangle();
636  return true;
637  }
638 
639  /***************************************************************************/
640  /* Tetrahedral and hexahedral versions of the metric. */
641  /***************************************************************************/
642 
643  inline bool m_gdft_3(double &obj,
644  const Vector3D x[4],
645  const Matrix3D &invR, /* upper triangular */
646  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
647  const double alpha = 1.0/3.0, /* constant */
648  const Exponent& gamma = 2.0/3.0, /* simplicial elements */
649  const double delta = 0.0,/* max in denominator */
650  const double beta = 0.0)/* no modification */
651  {
652  double matr[9], f, t1, t2;
653  double matd[9], g;
654 
655  /* Calculate M = A*inv(R). */
656  f = x[1][0] - x[0][0];
657  g = x[2][0] - x[0][0];
658  t1 = x[3][0] - x[0][0];
659  matr[0] = f*invR[0][0];
660  matr[1] = f*invR[0][1] + g*invR[1][1];
661  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
662 
663  f = x[1][1] - x[0][1];
664  g = x[2][1] - x[0][1];
665  t1 = x[3][1] - x[0][1];
666  matr[3] = f*invR[0][0];
667  matr[4] = f*invR[0][1] + g*invR[1][1];
668  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
669 
670  f = x[1][2] - x[0][2];
671  g = x[2][2] - x[0][2];
672  t1 = x[3][2] - x[0][2];
673  matr[6] = f*invR[0][0];
674  matr[7] = f*invR[0][1] + g*invR[1][1];
675  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
676 
677  /* Calculate det(M). */
678  t1 = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
679  matr[3]*(matr[2]*matr[7] - matr[1]*matr[8]) +
680  matr[6]*(matr[1]*matr[5] - matr[2]*matr[4]);
681 
682  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
683 
684  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
685  t2 = sqrt(t1*t1 + 4.0*delta*delta);
686  g = t1 + t2;
687 
688  /* Calculate N = M - beta*Q. */
689  matd[0] = matr[0] - beta*Q[0][0];
690  matd[1] = matr[1] - beta*Q[0][1];
691  matd[2] = matr[2] - beta*Q[0][2];
692  matd[3] = matr[3] - beta*Q[1][0];
693  matd[4] = matr[4] - beta*Q[1][1];
694  matd[5] = matr[5] - beta*Q[1][2];
695  matd[6] = matr[6] - beta*Q[2][0];
696  matd[7] = matr[7] - beta*Q[2][1];
697  matd[8] = matr[8] - beta*Q[2][2];
698 
699  /* Calculate norm(N) */
700  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
701  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
702  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
703 
704  /* Calculate objective function. */
705  obj = alpha * pow(2.0, gamma) * f / pow(g, gamma);
706  return true;
707  }
708 
709  inline bool g_gdft_3(double &obj, Vector3D g_obj[4],
710  const Vector3D x[4],
711  const Matrix3D &invR, /* upper triangular */
712  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
713  const double alpha = 1.0/3.0, /* constant */
714  const Exponent& gamma = 2.0/3.0, /* simplicial elements */
715  const double delta = 0.0,/* max in denominator */
716  const double beta = 0.0)/* no modification */
717  {
718  double matr[9], f, t1, t2;
719  double matd[9], g;
720  double adjm[9], loc1, loc2, loc3, loc4;
721 
722  /* Calculate M = A*inv(R). */
723  f = x[1][0] - x[0][0];
724  g = x[2][0] - x[0][0];
725  t1 = x[3][0] - x[0][0];
726  matr[0] = f*invR[0][0];
727  matr[1] = f*invR[0][1] + g*invR[1][1];
728  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
729 
730  f = x[1][1] - x[0][1];
731  g = x[2][1] - x[0][1];
732  t1 = x[3][1] - x[0][1];
733  matr[3] = f*invR[0][0];
734  matr[4] = f*invR[0][1] + g*invR[1][1];
735  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
736 
737  f = x[1][2] - x[0][2];
738  g = x[2][2] - x[0][2];
739  t1 = x[3][2] - x[0][2];
740  matr[6] = f*invR[0][0];
741  matr[7] = f*invR[0][1] + g*invR[1][1];
742  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
743 
744  /* Calculate det(M). */
745  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
746  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
747  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
748  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
749 
750  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
751 
752  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
753  t2 = sqrt(t1*t1 + 4.0*delta*delta);
754  g = t1 + t2;
755 
756  /* Calculate N = M - beta*Q. */
757  matd[0] = matr[0] - beta*Q[0][0];
758  matd[1] = matr[1] - beta*Q[0][1];
759  matd[2] = matr[2] - beta*Q[0][2];
760  matd[3] = matr[3] - beta*Q[1][0];
761  matd[4] = matr[4] - beta*Q[1][1];
762  matd[5] = matr[5] - beta*Q[1][2];
763  matd[6] = matr[6] - beta*Q[2][0];
764  matd[7] = matr[7] - beta*Q[2][1];
765  matd[8] = matr[8] - beta*Q[2][2];
766 
767  /* Calculate norm(N) */
768  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
769  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
770  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
771 
772  /* Calculate objective function. */
773  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
774  obj = f * loc4;
775 
776  /* Calculate the derivative of the objective function. */
777  f = 2.0 * loc4;
778  g = -gamma * obj / t2;
779 
780  /* Calculate adjoint matrix */
781  adjm[0] = f*matd[0] + g*loc1;
782  adjm[1] = f*matd[1] + g*loc2;
783  adjm[2] = f*matd[2] + g*loc3;
784 
785  loc1 = g*matr[0];
786  loc2 = g*matr[1];
787  loc3 = g*matr[2];
788 
789  adjm[3] = f*matd[3] + loc3*matr[7] - loc2*matr[8];
790  adjm[4] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
791  adjm[5] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
792 
793  adjm[6] = f*matd[6] + loc2*matr[5] - loc3*matr[4];
794  adjm[7] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
795  adjm[8] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
796 
797  /* Construct gradients */
798  g_obj[1][0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
799  g_obj[2][0] = invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
800  g_obj[3][0] = invR[2][2]*adjm[2];
801  g_obj[0][0] = -g_obj[1][0] - g_obj[2][0] - g_obj[3][0];
802 
803  g_obj[1][1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
804  g_obj[2][1] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
805  g_obj[3][1] = invR[2][2]*adjm[5];
806  g_obj[0][1] = -g_obj[1][1] - g_obj[2][1] - g_obj[3][1];
807 
808  g_obj[1][2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
809  g_obj[2][2] = invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
810  g_obj[3][2] = invR[2][2]*adjm[8];
811  g_obj[0][2] = -g_obj[1][2] - g_obj[2][2] - g_obj[3][2];
812  return true;
813  }
814 
815  inline bool h_gdft_3(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10],
816  const Vector3D x[4],
817  const Matrix3D &invR, /* upper triangular */
818  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
819  const double alpha = 1.0/3.0, /* constant */
820  const Exponent& gamma = 2.0/3.0, /* simplicial elements */
821  const double delta = 0.0,/* max in denominator */
822  const double beta = 0.0)/* no modification */
823  {
824  double matr[9], f, t1, t2;
825  double matd[9], g, t3, loc1;
826  double adjm[9], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
827  double J_A[6], J_B[10], J_C[10], J_D[6], J_E[10], J_F[6];
828  double A[12];
829 
830  /* Calculate M = A*inv(R). */
831  f = x[1][0] - x[0][0];
832  g = x[2][0] - x[0][0];
833  t1 = x[3][0] - x[0][0];
834  matr[0] = f*invR[0][0];
835  matr[1] = f*invR[0][1] + g*invR[1][1];
836  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
837 
838  f = x[1][1] - x[0][1];
839  g = x[2][1] - x[0][1];
840  t1 = x[3][1] - x[0][1];
841  matr[3] = f*invR[0][0];
842  matr[4] = f*invR[0][1] + g*invR[1][1];
843  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
844 
845  f = x[1][2] - x[0][2];
846  g = x[2][2] - x[0][2];
847  t1 = x[3][2] - x[0][2];
848  matr[6] = f*invR[0][0];
849  matr[7] = f*invR[0][1] + g*invR[1][1];
850  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
851 
852  /* Calculate det(M). */
853  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
854  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
855  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
856  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
857  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
858  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
859  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
860  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
861  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
862 
863  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
864 
865  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
866 
867  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
868  t2 = t1*t1 + 4.0*delta*delta;
869  t3 = sqrt(t2);
870  g = t1 + t3;
871 
872  /* Calculate N = M - beta*Q. */
873  matd[0] = matr[0] - beta*Q[0][0];
874  matd[1] = matr[1] - beta*Q[0][1];
875  matd[2] = matr[2] - beta*Q[0][2];
876  matd[3] = matr[3] - beta*Q[1][0];
877  matd[4] = matr[4] - beta*Q[1][1];
878  matd[5] = matr[5] - beta*Q[1][2];
879  matd[6] = matr[6] - beta*Q[2][0];
880  matd[7] = matr[7] - beta*Q[2][1];
881  matd[8] = matr[8] - beta*Q[2][2];
882 
883  /* Calculate norm(N) */
884  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
885  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
886  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
887 
888  /* Calculate objective function. */
889  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
890  obj = f * loc1;
891 
892  /* Calculate the derivative of the objective function. */
893  t3 = 1.0 / t3;
894  dobj_df = 2.0 * loc1;
895  dobj_dg = -gamma * obj * t3;
896  dobj_dfdg = -gamma * dobj_df * t3;
897  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
898 
899  /* Calculate adjoint matrix */
900  adjm[0] = dobj_df*matd[0] + dobj_dg*dg[0];
901  adjm[1] = dobj_df*matd[1] + dobj_dg*dg[1];
902  adjm[2] = dobj_df*matd[2] + dobj_dg*dg[2];
903  adjm[3] = dobj_df*matd[3] + dobj_dg*dg[3];
904  adjm[4] = dobj_df*matd[4] + dobj_dg*dg[4];
905  adjm[5] = dobj_df*matd[5] + dobj_dg*dg[5];
906  adjm[6] = dobj_df*matd[6] + dobj_dg*dg[6];
907  adjm[7] = dobj_df*matd[7] + dobj_dg*dg[7];
908  adjm[8] = dobj_df*matd[8] + dobj_dg*dg[8];
909 
910  /* Construct gradients */
911  g_obj[1][0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
912  g_obj[2][0] = invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
913  g_obj[3][0] = invR[2][2]*adjm[2];
914  g_obj[0][0] = -g_obj[1][0] - g_obj[2][0] - g_obj[3][0];
915 
916  g_obj[1][1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
917  g_obj[2][1] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
918  g_obj[3][1] = invR[2][2]*adjm[5];
919  g_obj[0][1] = -g_obj[1][1] - g_obj[2][1] - g_obj[3][1];
920 
921  g_obj[1][2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
922  g_obj[2][2] = invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
923  g_obj[3][2] = invR[2][2]*adjm[8];
924  g_obj[0][2] = -g_obj[1][2] - g_obj[2][2] - g_obj[3][2];
925 
926  /* Start of the Hessian evaluation */
927  adjm[0] = dobj_dg*matr[0]; matd[0] *= dobj_dfdg;
928  adjm[1] = dobj_dg*matr[1]; matd[1] *= dobj_dfdg;
929  adjm[2] = dobj_dg*matr[2]; matd[2] *= dobj_dfdg;
930  adjm[3] = dobj_dg*matr[3]; matd[3] *= dobj_dfdg;
931  adjm[4] = dobj_dg*matr[4]; matd[4] *= dobj_dfdg;
932  adjm[5] = dobj_dg*matr[5]; matd[5] *= dobj_dfdg;
933  adjm[6] = dobj_dg*matr[6]; matd[6] *= dobj_dfdg;
934  adjm[7] = dobj_dg*matr[7]; matd[7] *= dobj_dfdg;
935  adjm[8] = dobj_dg*matr[8]; matd[8] *= dobj_dfdg;
936 
937  /* Blocks for the Hessian construction */
938  loc1 = dobj_dgdg*dg[0] + matd[0];
939  J_A[0] = dobj_df + dg[0]*(matd[0] + loc1);
940  J_A[1] = dg[0]*matd[1] + loc1*dg[1];
941  J_A[2] = dg[0]*matd[2] + loc1*dg[2];
942  J_B[0] = dg[0]*matd[3] + loc1*dg[3];
943  J_B[1] = dg[0]*matd[4] + loc1*dg[4] + adjm[8];
944  J_B[2] = dg[0]*matd[5] + loc1*dg[5] - adjm[7];
945  J_C[0] = dg[0]*matd[6] + loc1*dg[6];
946  J_C[1] = dg[0]*matd[7] + loc1*dg[7] - adjm[5];
947  J_C[2] = dg[0]*matd[8] + loc1*dg[8] + adjm[4];
948 
949  loc1 = dobj_dgdg*dg[1] + matd[1];
950  J_A[3] = dobj_df + dg[1]*(matd[1] + loc1);
951  J_A[4] = dg[1]*matd[2] + loc1*dg[2];
952  J_B[3] = dg[1]*matd[3] + loc1*dg[3] - adjm[8];
953  J_B[4] = dg[1]*matd[4] + loc1*dg[4];
954  J_B[5] = dg[1]*matd[5] + loc1*dg[5] + adjm[6];
955  J_C[3] = dg[1]*matd[6] + loc1*dg[6] + adjm[5];
956  J_C[4] = dg[1]*matd[7] + loc1*dg[7];
957  J_C[5] = dg[1]*matd[8] + loc1*dg[8] - adjm[3];
958 
959  loc1 = dobj_dgdg*dg[2] + matd[2];
960  J_A[5] = dobj_df + dg[2]*(matd[2] + loc1);
961  J_B[6] = dg[2]*matd[3] + loc1*dg[3] + adjm[7];
962  J_B[7] = dg[2]*matd[4] + loc1*dg[4] - adjm[6];
963  J_B[8] = dg[2]*matd[5] + loc1*dg[5];
964  J_C[6] = dg[2]*matd[6] + loc1*dg[6] - adjm[4];
965  J_C[7] = dg[2]*matd[7] + loc1*dg[7] + adjm[3];
966  J_C[8] = dg[2]*matd[8] + loc1*dg[8];
967 
968  loc1 = dobj_dgdg*dg[3] + matd[3];
969  J_D[0] = dobj_df + dg[3]*(matd[3] + loc1);
970  J_D[1] = dg[3]*matd[4] + loc1*dg[4];
971  J_D[2] = dg[3]*matd[5] + loc1*dg[5];
972  J_E[0] = dg[3]*matd[6] + loc1*dg[6];
973  J_E[1] = dg[3]*matd[7] + loc1*dg[7] + adjm[2];
974  J_E[2] = dg[3]*matd[8] + loc1*dg[8] - adjm[1];
975 
976  loc1 = dobj_dgdg*dg[4] + matd[4];
977  J_D[3] = dobj_df + dg[4]*(matd[4] + loc1);
978  J_D[4] = dg[4]*matd[5] + loc1*dg[5];
979  J_E[3] = dg[4]*matd[6] + loc1*dg[6] - adjm[2];
980  J_E[4] = dg[4]*matd[7] + loc1*dg[7];
981  J_E[5] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
982 
983  loc1 = dobj_dgdg*dg[5] + matd[5];
984  J_D[5] = dobj_df + dg[5]*(matd[5] + loc1);
985  J_E[6] = dg[5]*matd[6] + loc1*dg[6] + adjm[1];
986  J_E[7] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
987  J_E[8] = dg[5]*matd[8] + loc1*dg[8];
988 
989  loc1 = dobj_dgdg*dg[6] + matd[6];
990  J_F[0] = dobj_df + dg[6]*(matd[6] + loc1);
991  J_F[1] = dg[6]*matd[7] + loc1*dg[7];
992  J_F[2] = dg[6]*matd[8] + loc1*dg[8];
993 
994  loc1 = dobj_dgdg*dg[7] + matd[7];
995  J_F[3] = dobj_df + dg[7]*(matd[7] + loc1);
996  J_F[4] = dg[7]*matd[8] + loc1*dg[8];
997 
998  J_F[5] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
999 
1000  /* Assemble matrix products */
1001 
1002  /* dx / dx */
1003  A[1] = J_A[0]*invR[0][0] + J_A[1]*invR[0][1] + J_A[2]*invR[0][2];
1004  A[2] = J_A[1]*invR[1][1] + J_A[2]*invR[1][2];
1005  A[3] = J_A[2]*invR[2][2];
1006  A[0] = -A[1] - A[2] - A[3];
1007 
1008  A[5] = J_A[1]*invR[0][0] + J_A[3]*invR[0][1] + J_A[4]*invR[0][2];
1009  A[6] = J_A[3]*invR[1][1] + J_A[4]*invR[1][2];
1010  A[7] = J_A[4]*invR[2][2];
1011  A[4] = -A[5] - A[6] - A[7];
1012 
1013  A[9] = J_A[2]*invR[0][0] + J_A[4]*invR[0][1] + J_A[5]*invR[0][2];
1014  A[10] = J_A[4]*invR[1][1] + J_A[5]*invR[1][2];
1015  A[11] = J_A[5]*invR[2][2];
1016  A[8] = -A[9] - A[10] - A[11];
1017 
1018  h_obj[1][0][0] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
1019  h_obj[2][0][0] = A[4]*invR[1][1] + A[8]*invR[1][2];
1020  h_obj[3][0][0] = A[8]*invR[2][2];
1021  h_obj[0][0][0] = -h_obj[1][0][0] - h_obj[2][0][0] - h_obj[3][0][0];
1022 
1023  h_obj[4][0][0] = A[1]*invR[0][0] + A[5]*invR[0][1] + A[9]*invR[0][2];
1024  h_obj[5][0][0] = A[5]*invR[1][1] + A[9]*invR[1][2];
1025  h_obj[6][0][0] = A[9]*invR[2][2];
1026 
1027  h_obj[7][0][0] = A[6]*invR[1][1] + A[10]*invR[1][2];
1028  h_obj[8][0][0] = A[10]*invR[2][2];
1029 
1030  h_obj[9][0][0] = A[11]*invR[2][2];
1031 
1032  /* dx / dy */
1033  A[1] = J_B[0]*invR[0][0] + J_B[1]*invR[0][1] + J_B[2]*invR[0][2];
1034  A[2] = J_B[1]*invR[1][1] + J_B[2]*invR[1][2];
1035  A[3] = J_B[2]*invR[2][2];
1036  A[0] = -A[1] - A[2] - A[3];
1037 
1038  A[5] = J_B[3]*invR[0][0] + J_B[4]*invR[0][1] + J_B[5]*invR[0][2];
1039  A[6] = J_B[4]*invR[1][1] + J_B[5]*invR[1][2];
1040  A[7] = J_B[5]*invR[2][2];
1041  A[4] = -A[5] - A[6] - A[7];
1042 
1043  A[9] = J_B[6]*invR[0][0] + J_B[7]*invR[0][1] + J_B[8]*invR[0][2];
1044  A[10] = J_B[7]*invR[1][1] + J_B[8]*invR[1][2];
1045  A[11] = J_B[8]*invR[2][2];
1046  A[8] = -A[9] - A[10] - A[11];
1047 
1048  h_obj[1][1][0] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
1049  h_obj[4][0][1] = A[1]*invR[0][0] + A[5]*invR[0][1] + A[9]*invR[0][2];
1050  h_obj[5][0][1] = A[2]*invR[0][0] + A[6]*invR[0][1] + A[10]*invR[0][2];
1051  h_obj[6][0][1] = A[3]*invR[0][0] + A[7]*invR[0][1] + A[11]*invR[0][2];
1052 
1053  h_obj[2][1][0] = A[4]*invR[1][1] + A[8]*invR[1][2];
1054  h_obj[5][1][0] = A[5]*invR[1][1] + A[9]*invR[1][2];
1055  h_obj[7][0][1] = A[6]*invR[1][1] + A[10]*invR[1][2];
1056  h_obj[8][0][1] = A[7]*invR[1][1] + A[11]*invR[1][2];
1057 
1058  h_obj[3][1][0] = A[8]*invR[2][2];
1059  h_obj[6][1][0] = A[9]*invR[2][2];
1060  h_obj[8][1][0] = A[10]*invR[2][2];
1061  h_obj[9][0][1] = A[11]*invR[2][2];
1062 
1063  h_obj[0][0][1] = -h_obj[1][1][0] - h_obj[2][1][0] - h_obj[3][1][0];
1064  h_obj[1][0][1] = -h_obj[4][0][1] - h_obj[5][1][0] - h_obj[6][1][0];
1065  h_obj[2][0][1] = -h_obj[5][0][1] - h_obj[7][0][1] - h_obj[8][1][0];
1066  h_obj[3][0][1] = -h_obj[6][0][1] - h_obj[8][0][1] - h_obj[9][0][1];
1067 
1068  /* dx / dz */
1069  A[1] = J_C[0]*invR[0][0] + J_C[1]*invR[0][1] + J_C[2]*invR[0][2];
1070  A[2] = J_C[1]*invR[1][1] + J_C[2]*invR[1][2];
1071  A[3] = J_C[2]*invR[2][2];
1072  A[0] = -A[1] - A[2] - A[3];
1073 
1074  A[5] = J_C[3]*invR[0][0] + J_C[4]*invR[0][1] + J_C[5]*invR[0][2];
1075  A[6] = J_C[4]*invR[1][1] + J_C[5]*invR[1][2];
1076  A[7] = J_C[5]*invR[2][2];
1077  A[4] = -A[5] - A[6] - A[7];
1078 
1079  A[9] = J_C[6]*invR[0][0] + J_C[7]*invR[0][1] + J_C[8]*invR[0][2];
1080  A[10] = J_C[7]*invR[1][1] + J_C[8]*invR[1][2];
1081  A[11] = J_C[8]*invR[2][2];
1082  A[8] = -A[9] - A[10] - A[11];
1083 
1084  h_obj[1][2][0] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
1085  h_obj[4][0][2] = A[1]*invR[0][0] + A[5]*invR[0][1] + A[9]*invR[0][2];
1086  h_obj[5][0][2] = A[2]*invR[0][0] + A[6]*invR[0][1] + A[10]*invR[0][2];
1087  h_obj[6][0][2] = A[3]*invR[0][0] + A[7]*invR[0][1] + A[11]*invR[0][2];
1088 
1089  h_obj[2][2][0] = A[4]*invR[1][1] + A[8]*invR[1][2];
1090  h_obj[5][2][0] = A[5]*invR[1][1] + A[9]*invR[1][2];
1091  h_obj[7][0][2] = A[6]*invR[1][1] + A[10]*invR[1][2];
1092  h_obj[8][0][2] = A[7]*invR[1][1] + A[11]*invR[1][2];
1093 
1094  h_obj[3][2][0] = A[8]*invR[2][2];
1095  h_obj[6][2][0] = A[9]*invR[2][2];
1096  h_obj[8][2][0] = A[10]*invR[2][2];
1097  h_obj[9][0][2] = A[11]*invR[2][2];
1098 
1099  h_obj[0][0][2] = -h_obj[1][2][0] - h_obj[2][2][0] - h_obj[3][2][0];
1100  h_obj[1][0][2] = -h_obj[4][0][2] - h_obj[5][2][0] - h_obj[6][2][0];
1101  h_obj[2][0][2] = -h_obj[5][0][2] - h_obj[7][0][2] - h_obj[8][2][0];
1102  h_obj[3][0][2] = -h_obj[6][0][2] - h_obj[8][0][2] - h_obj[9][0][2];
1103 
1104  /* dy / dy */
1105  A[1] = J_D[0]*invR[0][0] + J_D[1]*invR[0][1] + J_D[2]*invR[0][2];
1106  A[2] = J_D[1]*invR[1][1] + J_D[2]*invR[1][2];
1107  A[3] = J_D[2]*invR[2][2];
1108  A[0] = -A[1] - A[2] - A[3];
1109 
1110  A[5] = J_D[1]*invR[0][0] + J_D[3]*invR[0][1] + J_D[4]*invR[0][2];
1111  A[6] = J_D[3]*invR[1][1] + J_D[4]*invR[1][2];
1112  A[7] = J_D[4]*invR[2][2];
1113  A[4] = -A[5] - A[6] - A[7];
1114 
1115  A[9] = J_D[2]*invR[0][0] + J_D[4]*invR[0][1] + J_D[5]*invR[0][2];
1116  A[10] = J_D[4]*invR[1][1] + J_D[5]*invR[1][2];
1117  A[11] = J_D[5]*invR[2][2];
1118  A[8] = -A[9] - A[10] - A[11];
1119 
1120  h_obj[1][1][1] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
1121  h_obj[2][1][1] = A[4]*invR[1][1] + A[8]*invR[1][2];
1122  h_obj[3][1][1] = A[8]*invR[2][2];
1123  h_obj[0][1][1] = -h_obj[1][1][1] - h_obj[2][1][1] - h_obj[3][1][1];
1124 
1125  h_obj[4][1][1] = A[1]*invR[0][0] + A[5]*invR[0][1] + A[9]*invR[0][2];
1126  h_obj[5][1][1] = A[5]*invR[1][1] + A[9]*invR[1][2];
1127  h_obj[6][1][1] = A[9]*invR[2][2];
1128 
1129  h_obj[7][1][1] = A[6]*invR[1][1] + A[10]*invR[1][2];
1130  h_obj[8][1][1] = A[10]*invR[2][2];
1131 
1132  h_obj[9][1][1] = A[11]*invR[2][2];
1133 
1134  /* dy / dz */
1135  A[1] = J_E[0]*invR[0][0] + J_E[1]*invR[0][1] + J_E[2]*invR[0][2];
1136  A[2] = J_E[1]*invR[1][1] + J_E[2]*invR[1][2];
1137  A[3] = J_E[2]*invR[2][2];
1138  A[0] = -A[1] - A[2] - A[3];
1139 
1140  A[5] = J_E[3]*invR[0][0] + J_E[4]*invR[0][1] + J_E[5]*invR[0][2];
1141  A[6] = J_E[4]*invR[1][1] + J_E[5]*invR[1][2];
1142  A[7] = J_E[5]*invR[2][2];
1143  A[4] = -A[5] - A[6] - A[7];
1144 
1145  A[9] = J_E[6]*invR[0][0] + J_E[7]*invR[0][1] + J_E[8]*invR[0][2];
1146  A[10] = J_E[7]*invR[1][1] + J_E[8]*invR[1][2];
1147  A[11] = J_E[8]*invR[2][2];
1148  A[8] = -A[9] - A[10] - A[11];
1149 
1150  h_obj[1][2][1] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
1151  h_obj[4][1][2] = A[1]*invR[0][0] + A[5]*invR[0][1] + A[9]*invR[0][2];
1152  h_obj[5][1][2] = A[2]*invR[0][0] + A[6]*invR[0][1] + A[10]*invR[0][2];
1153  h_obj[6][1][2] = A[3]*invR[0][0] + A[7]*invR[0][1] + A[11]*invR[0][2];
1154 
1155  h_obj[2][2][1] = A[4]*invR[1][1] + A[8]*invR[1][2];
1156  h_obj[5][2][1] = A[5]*invR[1][1] + A[9]*invR[1][2];
1157  h_obj[7][1][2] = A[6]*invR[1][1] + A[10]*invR[1][2];
1158  h_obj[8][1][2] = A[7]*invR[1][1] + A[11]*invR[1][2];
1159 
1160  h_obj[3][2][1] = A[8]*invR[2][2];
1161  h_obj[6][2][1] = A[9]*invR[2][2];
1162  h_obj[8][2][1] = A[10]*invR[2][2];
1163  h_obj[9][1][2] = A[11]*invR[2][2];
1164 
1165  h_obj[0][1][2] = -h_obj[1][2][1] - h_obj[2][2][1] - h_obj[3][2][1];
1166  h_obj[1][1][2] = -h_obj[4][1][2] - h_obj[5][2][1] - h_obj[6][2][1];
1167  h_obj[2][1][2] = -h_obj[5][1][2] - h_obj[7][1][2] - h_obj[8][2][1];
1168  h_obj[3][1][2] = -h_obj[6][1][2] - h_obj[8][1][2] - h_obj[9][1][2];
1169 
1170  /* dz / dz */
1171  A[1] = J_F[0]*invR[0][0] + J_F[1]*invR[0][1] + J_F[2]*invR[0][2];
1172  A[2] = J_F[1]*invR[1][1] + J_F[2]*invR[1][2];
1173  A[3] = J_F[2]*invR[2][2];
1174  A[0] = -A[1] - A[2] - A[3];
1175 
1176  A[5] = J_F[1]*invR[0][0] + J_F[3]*invR[0][1] + J_F[4]*invR[0][2];
1177  A[6] = J_F[3]*invR[1][1] + J_F[4]*invR[1][2];
1178  A[7] = J_F[4]*invR[2][2];
1179  A[4] = -A[5] - A[6] - A[7];
1180 
1181  A[9] = J_F[2]*invR[0][0] + J_F[4]*invR[0][1] + J_F[5]*invR[0][2];
1182  A[10] = J_F[4]*invR[1][1] + J_F[5]*invR[1][2];
1183  A[11] = J_F[5]*invR[2][2];
1184  A[8] = -A[9] - A[10] - A[11];
1185 
1186  h_obj[1][2][2] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
1187  h_obj[2][2][2] = A[4]*invR[1][1] + A[8]*invR[1][2];
1188  h_obj[3][2][2] = A[8]*invR[2][2];
1189  h_obj[0][2][2] = -h_obj[1][2][2] - h_obj[2][2][2] - h_obj[3][2][2];
1190 
1191  h_obj[4][2][2] = A[1]*invR[0][0] + A[5]*invR[0][1] + A[9]*invR[0][2];
1192  h_obj[5][2][2] = A[5]*invR[1][1] + A[9]*invR[1][2];
1193  h_obj[6][2][2] = A[9]*invR[2][2];
1194 
1195  h_obj[7][2][2] = A[6]*invR[1][1] + A[10]*invR[1][2];
1196  h_obj[8][2][2] = A[10]*invR[2][2];
1197 
1198  h_obj[9][2][2] = A[11]*invR[2][2];
1199 
1200  /* Complete diagonal blocks */
1201  h_obj[0].fill_lower_triangle();
1202  h_obj[4].fill_lower_triangle();
1203  h_obj[7].fill_lower_triangle();
1204  h_obj[9].fill_lower_triangle();
1205  return true;
1206  }
1207 
1208  /***************************************************************************/
1209  /* The following code computes the gradient and Hessian with respect to a */
1210  /* particular vertex. These specializations are required to get good */
1211  /* performance out of a local code. The fastest versions would need to */
1212  /* do store a seperate version of invR and Q for each vertex, which is not */
1213  /* an efficient use of the storage. */
1214  /***************************************************************************/
1215 
1216  inline bool g_gdft_2_v0(double &obj, Vector3D &g_obj,
1217  const Vector3D x[3], const Vector3D &n,
1218  const Matrix3D &invR, /* upper triangular */
1219  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
1220  const double alpha = 0.5,/* constant */
1221  const Exponent& gamma = 1.0,/* simplicial elements */
1222  const double delta = 0.0,/* max in denominator */
1223  const double beta = 0.0)/* no modification */
1224  {
1225  double matr[9], f, t1, t2;
1226  double matd[9], g;
1227  double adjm[9], loc1, loc2, loc3, loc4;
1228 
1229  /* Calculate M = A*inv(R). */
1230  f = x[1][0] - x[0][0];
1231  g = x[2][0] - x[0][0];
1232  matr[0] = f*invR[0][0];
1233  matr[1] = f*invR[0][1] + g*invR[1][1];
1234  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
1235 
1236  f = x[1][1] - x[0][1];
1237  g = x[2][1] - x[0][1];
1238  matr[3] = f*invR[0][0];
1239  matr[4] = f*invR[0][1] + g*invR[1][1];
1240  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
1241 
1242  f = x[1][2] - x[0][2];
1243  g = x[2][2] - x[0][2];
1244  matr[6] = f*invR[0][0];
1245  matr[7] = f*invR[0][1] + g*invR[1][1];
1246  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
1247 
1248  /* Calculate det(M). */
1249  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
1250  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
1251  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
1252  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
1253 
1254  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
1255 
1256  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
1257  t2 = sqrt(t1*t1 + 4.0*delta*delta);
1258  g = t1 + t2;
1259 
1260  /* Calculate N = M - beta*Q. */
1261  matd[0] = matr[0] - beta*Q[0][0];
1262  matd[1] = matr[1] - beta*Q[0][1];
1263  matd[2] = matr[2] - beta*Q[0][2];
1264  matd[3] = matr[3] - beta*Q[1][0];
1265  matd[4] = matr[4] - beta*Q[1][1];
1266  matd[5] = matr[5] - beta*Q[1][2];
1267  matd[6] = matr[6] - beta*Q[2][0];
1268  matd[7] = matr[7] - beta*Q[2][1];
1269  matd[8] = matr[8] - beta*Q[2][2];
1270 
1271  /* Calculate norm(N) */
1272  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
1273  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
1274  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
1275 
1276  /* Calculate objective function. */
1277  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
1278  obj = f * loc4;
1279 
1280  /* Calculate the derivative of the objective function. */
1281  f = 2.0 * loc4;
1282  g = -gamma * obj / t2;
1283 
1284  /* Calculate adjoint matrix */
1285  adjm[0] = f*matd[0] + g*loc1;
1286  adjm[1] = f*matd[1] + g*loc2;
1287  adjm[2] = f*matd[2] + g*loc3;
1288 
1289  loc1 = g*matr[0];
1290  loc2 = g*matr[1];
1291  loc3 = g*matr[2];
1292 
1293  adjm[3] = f*matd[3] + loc3*matr[7] - loc2*matr[8];
1294  adjm[4] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
1295  adjm[5] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
1296 
1297  adjm[6] = f*matd[6] + loc2*matr[5] - loc3*matr[4];
1298  adjm[7] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
1299  adjm[8] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
1300 
1301  /* Construct gradients */
1302  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
1303  g_obj[0] += invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
1304  g_obj[0] = -g_obj[0];
1305 
1306  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
1307  g_obj[1] += invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
1308  g_obj[1] = -g_obj[1];
1309 
1310  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
1311  g_obj[2] += invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
1312  g_obj[2] = -g_obj[2];
1313  return true;
1314  }
1315 
1316  inline bool g_gdft_2_v1(double &obj, Vector3D &g_obj,
1317  const Vector3D x[3], const Vector3D &n,
1318  const Matrix3D &invR, /* upper triangular */
1319  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
1320  const double alpha = 0.5,/* constant */
1321  const Exponent& gamma = 1.0,/* simplicial elements */
1322  const double delta = 0.0,/* max in denominator */
1323  const double beta = 0.0)/* no modification */
1324  {
1325  double matr[9], f, t1, t2;
1326  double matd[9], g;
1327  double adjm[9], loc1, loc2, loc3, loc4;
1328 
1329  /* Calculate M = A*inv(R). */
1330  f = x[1][0] - x[0][0];
1331  g = x[2][0] - x[0][0];
1332  matr[0] = f*invR[0][0];
1333  matr[1] = f*invR[0][1] + g*invR[1][1];
1334  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
1335 
1336  f = x[1][1] - x[0][1];
1337  g = x[2][1] - x[0][1];
1338  matr[3] = f*invR[0][0];
1339  matr[4] = f*invR[0][1] + g*invR[1][1];
1340  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
1341 
1342  f = x[1][2] - x[0][2];
1343  g = x[2][2] - x[0][2];
1344  matr[6] = f*invR[0][0];
1345  matr[7] = f*invR[0][1] + g*invR[1][1];
1346  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
1347 
1348  /* Calculate det(M). */
1349  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
1350  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
1351  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
1352  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
1353 
1354  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
1355 
1356  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
1357  t2 = sqrt(t1*t1 + 4.0*delta*delta);
1358  g = t1 + t2;
1359 
1360  /* Calculate N = M - beta*Q. */
1361  matd[0] = matr[0] - beta*Q[0][0];
1362  matd[1] = matr[1] - beta*Q[0][1];
1363  matd[2] = matr[2] - beta*Q[0][2];
1364  matd[3] = matr[3] - beta*Q[1][0];
1365  matd[4] = matr[4] - beta*Q[1][1];
1366  matd[5] = matr[5] - beta*Q[1][2];
1367  matd[6] = matr[6] - beta*Q[2][0];
1368  matd[7] = matr[7] - beta*Q[2][1];
1369  matd[8] = matr[8] - beta*Q[2][2];
1370 
1371  /* Calculate norm(N) */
1372  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
1373  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
1374  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
1375 
1376  /* Calculate objective function. */
1377  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
1378  obj = f * loc4;
1379 
1380  /* Calculate the derivative of the objective function. */
1381  f = 2.0 * loc4;
1382  g = -gamma * obj / t2;
1383 
1384  /* Calculate adjoint matrix */
1385  adjm[0] = f*matd[0] + g*loc1;
1386  adjm[1] = f*matd[1] + g*loc2;
1387  adjm[2] = f*matd[2] + g*loc3;
1388 
1389  loc1 = g*matr[0];
1390  loc2 = g*matr[1];
1391  loc3 = g*matr[2];
1392 
1393  adjm[3] = f*matd[3] + loc3*matr[7] - loc2*matr[8];
1394  adjm[4] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
1395  adjm[5] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
1396 
1397  adjm[6] = f*matd[6] + loc2*matr[5] - loc3*matr[4];
1398  adjm[7] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
1399  adjm[8] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
1400 
1401  /* Construct gradients */
1402  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
1403  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
1404  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
1405  return true;
1406  }
1407 
1408  inline bool g_gdft_2_v2(double &obj, Vector3D &g_obj,
1409  const Vector3D x[3], const Vector3D &n,
1410  const Matrix3D &invR, /* upper triangular */
1411  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
1412  const double alpha = 0.5,/* constant */
1413  const Exponent& gamma = 1.0,/* simplicial elements */
1414  const double delta = 0.0,/* max in denominator */
1415  const double beta = 0.0)/* no modification */
1416  {
1417  double matr[9], f, t1, t2;
1418  double matd[9], g;
1419  double adjm[6], loc1, loc2, loc3, loc4;
1420 
1421  /* Calculate M = A*inv(R). */
1422  f = x[1][0] - x[0][0];
1423  g = x[2][0] - x[0][0];
1424  matr[0] = f*invR[0][0];
1425  matr[1] = f*invR[0][1] + g*invR[1][1];
1426  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
1427 
1428  f = x[1][1] - x[0][1];
1429  g = x[2][1] - x[0][1];
1430  matr[3] = f*invR[0][0];
1431  matr[4] = f*invR[0][1] + g*invR[1][1];
1432  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
1433 
1434  f = x[1][2] - x[0][2];
1435  g = x[2][2] - x[0][2];
1436  matr[6] = f*invR[0][0];
1437  matr[7] = f*invR[0][1] + g*invR[1][1];
1438  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
1439 
1440  /* Calculate det(M). */
1441  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
1442  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
1443  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
1444  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
1445 
1446  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
1447 
1448  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
1449  t2 = sqrt(t1*t1 + 4.0*delta*delta);
1450  g = t1 + t2;
1451 
1452  /* Calculate N = M - beta*Q. */
1453  matd[0] = matr[0] - beta*Q[0][0];
1454  matd[1] = matr[1] - beta*Q[0][1];
1455  matd[2] = matr[2] - beta*Q[0][2];
1456  matd[3] = matr[3] - beta*Q[1][0];
1457  matd[4] = matr[4] - beta*Q[1][1];
1458  matd[5] = matr[5] - beta*Q[1][2];
1459  matd[6] = matr[6] - beta*Q[2][0];
1460  matd[7] = matr[7] - beta*Q[2][1];
1461  matd[8] = matr[8] - beta*Q[2][2];
1462 
1463  /* Calculate norm(N) */
1464  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
1465  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
1466  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
1467 
1468  /* Calculate objective function. */
1469  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
1470  obj = f * loc4;
1471 
1472  /* Calculate the derivative of the objective function. */
1473  f = 2.0 * loc4;
1474  g = -gamma * obj / t2;
1475 
1476  /* Calculate adjoint matrix */
1477  adjm[0] = f*matd[1] + g*loc2;
1478  adjm[1] = f*matd[2] + g*loc3;
1479 
1480  loc1 = g*matr[0];
1481  loc2 = g*matr[1];
1482  loc3 = g*matr[2];
1483 
1484  adjm[2] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
1485  adjm[3] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
1486 
1487  adjm[4] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
1488  adjm[5] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
1489 
1490  /* Construct gradients */
1491  g_obj[0] = invR[1][1]*adjm[0]+invR[1][2]*adjm[1];
1492  g_obj[1] = invR[1][1]*adjm[2]+invR[1][2]*adjm[3];
1493  g_obj[2] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
1494  return true;
1495  }
1496 
1497  inline bool h_gdft_2_v0(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
1498  const Vector3D x[3], const Vector3D &n,
1499  const Matrix3D &invR, /* upper triangular */
1500  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
1501  const double alpha = 0.5,/* constant */
1502  const Exponent& gamma = 1.0,/* simplicial elements */
1503  const double delta = 0.0,/* max in denominator */
1504  const double beta = 0.0)/* no modification */
1505  {
1506  double matr[9], f, t1, t2;
1507  double matd[9], g, t3, loc1;
1508  double adjm[9], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
1509  double J_A[6], J_B[10], J_C[10], J_D[6], J_E[10], J_F[6];
1510  double A[9];
1511 
1512  /* Calculate M = A*inv(R). */
1513  f = x[1][0] - x[0][0];
1514  g = x[2][0] - x[0][0];
1515  matr[0] = f*invR[0][0];
1516  matr[1] = f*invR[0][1] + g*invR[1][1];
1517  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
1518 
1519  f = x[1][1] - x[0][1];
1520  g = x[2][1] - x[0][1];
1521  matr[3] = f*invR[0][0];
1522  matr[4] = f*invR[0][1] + g*invR[1][1];
1523  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
1524 
1525  f = x[1][2] - x[0][2];
1526  g = x[2][2] - x[0][2];
1527  matr[6] = f*invR[0][0];
1528  matr[7] = f*invR[0][1] + g*invR[1][1];
1529  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
1530 
1531  /* Calculate det(M). */
1532  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
1533  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
1534  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
1535  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
1536  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
1537  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
1538  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
1539  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
1540  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
1541 
1542  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
1543 
1544  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
1545 
1546  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
1547  t2 = t1*t1 + 4.0*delta*delta;
1548  t3 = sqrt(t2);
1549  g = t1 + t3;
1550 
1551  /* Calculate N = M - beta*Q. */
1552  matd[0] = matr[0] - beta*Q[0][0];
1553  matd[1] = matr[1] - beta*Q[0][1];
1554  matd[2] = matr[2] - beta*Q[0][2];
1555  matd[3] = matr[3] - beta*Q[1][0];
1556  matd[4] = matr[4] - beta*Q[1][1];
1557  matd[5] = matr[5] - beta*Q[1][2];
1558  matd[6] = matr[6] - beta*Q[2][0];
1559  matd[7] = matr[7] - beta*Q[2][1];
1560  matd[8] = matr[8] - beta*Q[2][2];
1561 
1562  /* Calculate norm(N) */
1563  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
1564  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
1565  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
1566 
1567  /* Calculate objective function. */
1568  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
1569  obj = f * loc1;
1570 
1571  /* Calculate the derivative of the objective function. */
1572  t3 = 1.0 / t3;
1573  dobj_df = 2.0 * loc1;
1574  dobj_dg = -gamma * obj * t3;
1575  dobj_dfdg = -gamma * dobj_df * t3;
1576  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
1577 
1578  /* Calculate adjoint matrix */
1579  adjm[0] = dobj_df*matd[0] + dobj_dg*dg[0];
1580  adjm[1] = dobj_df*matd[1] + dobj_dg*dg[1];
1581  adjm[2] = dobj_df*matd[2] + dobj_dg*dg[2];
1582  adjm[3] = dobj_df*matd[3] + dobj_dg*dg[3];
1583  adjm[4] = dobj_df*matd[4] + dobj_dg*dg[4];
1584  adjm[5] = dobj_df*matd[5] + dobj_dg*dg[5];
1585  adjm[6] = dobj_df*matd[6] + dobj_dg*dg[6];
1586  adjm[7] = dobj_df*matd[7] + dobj_dg*dg[7];
1587  adjm[8] = dobj_df*matd[8] + dobj_dg*dg[8];
1588 
1589  /* Construct gradients */
1590  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
1591  g_obj[0] += invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
1592  g_obj[0] = -g_obj[0];
1593 
1594  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
1595  g_obj[1] += invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
1596  g_obj[1] = -g_obj[1];
1597 
1598  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
1599  g_obj[2] += invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
1600  g_obj[2] = -g_obj[2];
1601 
1602  /* Start of the Hessian evaluation */
1603  adjm[0] = dobj_dg*matr[0]; matd[0] *= dobj_dfdg;
1604  adjm[1] = dobj_dg*matr[1]; matd[1] *= dobj_dfdg;
1605  adjm[2] = dobj_dg*matr[2]; matd[2] *= dobj_dfdg;
1606  adjm[3] = dobj_dg*matr[3]; matd[3] *= dobj_dfdg;
1607  adjm[4] = dobj_dg*matr[4]; matd[4] *= dobj_dfdg;
1608  adjm[5] = dobj_dg*matr[5]; matd[5] *= dobj_dfdg;
1609  adjm[6] = dobj_dg*matr[6]; matd[6] *= dobj_dfdg;
1610  adjm[7] = dobj_dg*matr[7]; matd[7] *= dobj_dfdg;
1611  adjm[8] = dobj_dg*matr[8]; matd[8] *= dobj_dfdg;
1612 
1613  /* Blocks for the Hessian construction */
1614  loc1 = dobj_dgdg*dg[0] + matd[0];
1615  J_A[0] = dobj_df + dg[0]*(matd[0] + loc1);
1616  J_A[1] = dg[0]*matd[1] + loc1*dg[1];
1617  J_A[2] = dg[0]*matd[2] + loc1*dg[2];
1618  J_B[0] = dg[0]*matd[3] + loc1*dg[3];
1619  J_B[1] = dg[0]*matd[4] + loc1*dg[4] + adjm[8];
1620  J_B[2] = dg[0]*matd[5] + loc1*dg[5] - adjm[7];
1621  J_C[0] = dg[0]*matd[6] + loc1*dg[6];
1622  J_C[1] = dg[0]*matd[7] + loc1*dg[7] - adjm[5];
1623  J_C[2] = dg[0]*matd[8] + loc1*dg[8] + adjm[4];
1624 
1625  loc1 = dobj_dgdg*dg[1] + matd[1];
1626  J_A[3] = dobj_df + dg[1]*(matd[1] + loc1);
1627  J_A[4] = dg[1]*matd[2] + loc1*dg[2];
1628  J_B[3] = dg[1]*matd[3] + loc1*dg[3] - adjm[8];
1629  J_B[4] = dg[1]*matd[4] + loc1*dg[4];
1630  J_B[5] = dg[1]*matd[5] + loc1*dg[5] + adjm[6];
1631  J_C[3] = dg[1]*matd[6] + loc1*dg[6] + adjm[5];
1632  J_C[4] = dg[1]*matd[7] + loc1*dg[7];
1633  J_C[5] = dg[1]*matd[8] + loc1*dg[8] - adjm[3];
1634 
1635  loc1 = dobj_dgdg*dg[2] + matd[2];
1636  J_A[5] = dobj_df + dg[2]*(matd[2] + loc1);
1637  J_B[6] = dg[2]*matd[3] + loc1*dg[3] + adjm[7];
1638  J_B[7] = dg[2]*matd[4] + loc1*dg[4] - adjm[6];
1639  J_B[8] = dg[2]*matd[5] + loc1*dg[5];
1640  J_C[6] = dg[2]*matd[6] + loc1*dg[6] - adjm[4];
1641  J_C[7] = dg[2]*matd[7] + loc1*dg[7] + adjm[3];
1642  J_C[8] = dg[2]*matd[8] + loc1*dg[8];
1643 
1644  loc1 = dobj_dgdg*dg[3] + matd[3];
1645  J_D[0] = dobj_df + dg[3]*(matd[3] + loc1);
1646  J_D[1] = dg[3]*matd[4] + loc1*dg[4];
1647  J_D[2] = dg[3]*matd[5] + loc1*dg[5];
1648  J_E[0] = dg[3]*matd[6] + loc1*dg[6];
1649  J_E[1] = dg[3]*matd[7] + loc1*dg[7] + adjm[2];
1650  J_E[2] = dg[3]*matd[8] + loc1*dg[8] - adjm[1];
1651 
1652  loc1 = dobj_dgdg*dg[4] + matd[4];
1653  J_D[3] = dobj_df + dg[4]*(matd[4] + loc1);
1654  J_D[4] = dg[4]*matd[5] + loc1*dg[5];
1655  J_E[3] = dg[4]*matd[6] + loc1*dg[6] - adjm[2];
1656  J_E[4] = dg[4]*matd[7] + loc1*dg[7];
1657  J_E[5] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
1658 
1659  loc1 = dobj_dgdg*dg[5] + matd[5];
1660  J_D[5] = dobj_df + dg[5]*(matd[5] + loc1);
1661  J_E[6] = dg[5]*matd[6] + loc1*dg[6] + adjm[1];
1662  J_E[7] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
1663  J_E[8] = dg[5]*matd[8] + loc1*dg[8];
1664 
1665  loc1 = dobj_dgdg*dg[6] + matd[6];
1666  J_F[0] = dobj_df + dg[6]*(matd[6] + loc1);
1667  J_F[1] = dg[6]*matd[7] + loc1*dg[7];
1668  J_F[2] = dg[6]*matd[8] + loc1*dg[8];
1669 
1670  loc1 = dobj_dgdg*dg[7] + matd[7];
1671  J_F[3] = dobj_df + dg[7]*(matd[7] + loc1);
1672  J_F[4] = dg[7]*matd[8] + loc1*dg[8];
1673 
1674  J_F[5] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
1675 
1676  /* Assemble matrix products */
1677 
1678  /* dx / dx */
1679  A[1] = J_A[0]*invR[0][0] + J_A[1]*invR[0][1] + J_A[2]*invR[0][2];
1680  A[2] = J_A[1]*invR[1][1] + J_A[2]*invR[1][2];
1681  A[0] = -A[1] - A[2];
1682 
1683  A[4] = J_A[1]*invR[0][0] + J_A[3]*invR[0][1] + J_A[4]*invR[0][2];
1684  A[5] = J_A[3]*invR[1][1] + J_A[4]*invR[1][2];
1685  A[3] = -A[4] - A[5];
1686 
1687  A[7] = J_A[2]*invR[0][0] + J_A[4]*invR[0][1] + J_A[5]*invR[0][2];
1688  A[8] = J_A[4]*invR[1][1] + J_A[5]*invR[1][2];
1689  A[6] = -A[7] - A[8];
1690 
1691  h_obj[0][0] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
1692  h_obj[0][0] += A[3]*invR[1][1] + A[6]*invR[1][2];
1693  h_obj[0][0] = -h_obj[0][0];
1694 
1695  /* dx / dy */
1696  A[1] = J_B[0]*invR[0][0] + J_B[1]*invR[0][1] + J_B[2]*invR[0][2];
1697  A[2] = J_B[1]*invR[1][1] + J_B[2]*invR[1][2];
1698  A[0] = -A[1] - A[2];
1699 
1700  A[4] = J_B[3]*invR[0][0] + J_B[4]*invR[0][1] + J_B[5]*invR[0][2];
1701  A[5] = J_B[4]*invR[1][1] + J_B[5]*invR[1][2];
1702  A[3] = -A[4] - A[5];
1703 
1704  A[7] = J_B[6]*invR[0][0] + J_B[7]*invR[0][1] + J_B[8]*invR[0][2];
1705  A[8] = J_B[7]*invR[1][1] + J_B[8]*invR[1][2];
1706  A[6] = -A[7] - A[8];
1707 
1708  h_obj[0][1] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
1709  h_obj[0][1] += A[3]*invR[1][1] + A[6]*invR[1][2];
1710  h_obj[0][1] = -h_obj[0][1];
1711 
1712  /* dx / dz */
1713  A[1] = J_C[0]*invR[0][0] + J_C[1]*invR[0][1] + J_C[2]*invR[0][2];
1714  A[2] = J_C[1]*invR[1][1] + J_C[2]*invR[1][2];
1715  A[0] = -A[1] - A[2];
1716 
1717  A[4] = J_C[3]*invR[0][0] + J_C[4]*invR[0][1] + J_C[5]*invR[0][2];
1718  A[5] = J_C[4]*invR[1][1] + J_C[5]*invR[1][2];
1719  A[3] = -A[4] - A[5];
1720 
1721  A[7] = J_C[6]*invR[0][0] + J_C[7]*invR[0][1] + J_C[8]*invR[0][2];
1722  A[8] = J_C[7]*invR[1][1] + J_C[8]*invR[1][2];
1723  A[6] = -A[7] - A[8];
1724 
1725  h_obj[0][2] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
1726  h_obj[0][2] += A[3]*invR[1][1] + A[6]*invR[1][2];
1727  h_obj[0][2] = -h_obj[0][2];
1728 
1729  /* dy / dy */
1730  A[1] = J_D[0]*invR[0][0] + J_D[1]*invR[0][1] + J_D[2]*invR[0][2];
1731  A[2] = J_D[1]*invR[1][1] + J_D[2]*invR[1][2];
1732  A[0] = -A[1] - A[2];
1733 
1734  A[4] = J_D[1]*invR[0][0] + J_D[3]*invR[0][1] + J_D[4]*invR[0][2];
1735  A[5] = J_D[3]*invR[1][1] + J_D[4]*invR[1][2];
1736  A[3] = -A[4] - A[5];
1737 
1738  A[7] = J_D[2]*invR[0][0] + J_D[4]*invR[0][1] + J_D[5]*invR[0][2];
1739  A[8] = J_D[4]*invR[1][1] + J_D[5]*invR[1][2];
1740  A[6] = -A[7] - A[8];
1741 
1742  h_obj[1][1] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
1743  h_obj[1][1] += A[3]*invR[1][1] + A[6]*invR[1][2];
1744  h_obj[1][1] = -h_obj[1][1];
1745 
1746  /* dy / dz */
1747  A[1] = J_E[0]*invR[0][0] + J_E[1]*invR[0][1] + J_E[2]*invR[0][2];
1748  A[2] = J_E[1]*invR[1][1] + J_E[2]*invR[1][2];
1749  A[0] = -A[1] - A[2];
1750 
1751  A[4] = J_E[3]*invR[0][0] + J_E[4]*invR[0][1] + J_E[5]*invR[0][2];
1752  A[5] = J_E[4]*invR[1][1] + J_E[5]*invR[1][2];
1753  A[3] = -A[4] - A[5];
1754 
1755  A[7] = J_E[6]*invR[0][0] + J_E[7]*invR[0][1] + J_E[8]*invR[0][2];
1756  A[8] = J_E[7]*invR[1][1] + J_E[8]*invR[1][2];
1757  A[6] = -A[7] - A[8];
1758 
1759  h_obj[1][2] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
1760  h_obj[1][2] += A[3]*invR[1][1] + A[6]*invR[1][2];
1761  h_obj[1][2] = -h_obj[1][2];
1762 
1763  /* dz / dz */
1764  A[1] = J_F[0]*invR[0][0] + J_F[1]*invR[0][1] + J_F[2]*invR[0][2];
1765  A[2] = J_F[1]*invR[1][1] + J_F[2]*invR[1][2];
1766  A[0] = -A[1] - A[2];
1767 
1768  A[4] = J_F[1]*invR[0][0] + J_F[3]*invR[0][1] + J_F[4]*invR[0][2];
1769  A[5] = J_F[3]*invR[1][1] + J_F[4]*invR[1][2];
1770  A[3] = -A[4] - A[5];
1771 
1772  A[7] = J_F[2]*invR[0][0] + J_F[4]*invR[0][1] + J_F[5]*invR[0][2];
1773  A[8] = J_F[4]*invR[1][1] + J_F[5]*invR[1][2];
1774  A[6] = -A[7] - A[8];
1775 
1776  h_obj[2][2] = A[0]*invR[0][0] + A[3]*invR[0][1] + A[6]*invR[0][2];
1777  h_obj[2][2] += A[3]*invR[1][1] + A[6]*invR[1][2];
1778  h_obj[2][2] = -h_obj[2][2];
1779 
1780  /* Complete diagonal blocks */
1781  h_obj.fill_lower_triangle();
1782  return true;
1783  }
1784 
1785  inline bool h_gdft_2_v1(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
1786  const Vector3D x[3], const Vector3D &n,
1787  const Matrix3D &invR, /* upper triangular */
1788  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
1789  const double alpha = 0.5,/* constant */
1790  const Exponent& gamma = 1.0,/* simplicial elements */
1791  const double delta = 0.0,/* max in denominator */
1792  const double beta = 0.0)/* no modification */
1793  {
1794  double matr[9], f, t1, t2;
1795  double matd[9], g, t3, loc1;
1796  double adjm[9], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
1797  double J_A[6], J_B[10], J_C[10], J_D[6], J_E[10], J_F[6];
1798  double A[3];
1799 
1800  /* Calculate M = A*inv(R). */
1801  f = x[1][0] - x[0][0];
1802  g = x[2][0] - x[0][0];
1803  matr[0] = f*invR[0][0];
1804  matr[1] = f*invR[0][1] + g*invR[1][1];
1805  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
1806 
1807  f = x[1][1] - x[0][1];
1808  g = x[2][1] - x[0][1];
1809  matr[3] = f*invR[0][0];
1810  matr[4] = f*invR[0][1] + g*invR[1][1];
1811  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
1812 
1813  f = x[1][2] - x[0][2];
1814  g = x[2][2] - x[0][2];
1815  matr[6] = f*invR[0][0];
1816  matr[7] = f*invR[0][1] + g*invR[1][1];
1817  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
1818 
1819  /* Calculate det(M). */
1820  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
1821  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
1822  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
1823  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
1824  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
1825  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
1826  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
1827  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
1828  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
1829 
1830  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
1831 
1832  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
1833 
1834  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
1835  t2 = t1*t1 + 4.0*delta*delta;
1836  t3 = sqrt(t2);
1837  g = t1 + t3;
1838 
1839  /* Calculate N = M - beta*Q. */
1840  matd[0] = matr[0] - beta*Q[0][0];
1841  matd[1] = matr[1] - beta*Q[0][1];
1842  matd[2] = matr[2] - beta*Q[0][2];
1843  matd[3] = matr[3] - beta*Q[1][0];
1844  matd[4] = matr[4] - beta*Q[1][1];
1845  matd[5] = matr[5] - beta*Q[1][2];
1846  matd[6] = matr[6] - beta*Q[2][0];
1847  matd[7] = matr[7] - beta*Q[2][1];
1848  matd[8] = matr[8] - beta*Q[2][2];
1849 
1850  /* Calculate norm(N) */
1851  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
1852  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
1853  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
1854 
1855  /* Calculate objective function. */
1856  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
1857  obj = f * loc1;
1858 
1859  /* Calculate the derivative of the objective function. */
1860  t3 = 1.0 / t3;
1861  dobj_df = 2.0 * loc1;
1862  dobj_dg = -gamma * obj * t3;
1863  dobj_dfdg = -gamma * dobj_df * t3;
1864  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
1865 
1866  /* Calculate adjoint matrix */
1867  adjm[0] = dobj_df*matd[0] + dobj_dg*dg[0];
1868  adjm[1] = dobj_df*matd[1] + dobj_dg*dg[1];
1869  adjm[2] = dobj_df*matd[2] + dobj_dg*dg[2];
1870  adjm[3] = dobj_df*matd[3] + dobj_dg*dg[3];
1871  adjm[4] = dobj_df*matd[4] + dobj_dg*dg[4];
1872  adjm[5] = dobj_df*matd[5] + dobj_dg*dg[5];
1873  adjm[6] = dobj_df*matd[6] + dobj_dg*dg[6];
1874  adjm[7] = dobj_df*matd[7] + dobj_dg*dg[7];
1875  adjm[8] = dobj_df*matd[8] + dobj_dg*dg[8];
1876 
1877  /* Construct gradients */
1878  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
1879  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
1880  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
1881 
1882  /* Start of the Hessian evaluation */
1883  adjm[0] = dobj_dg*matr[0]; matd[0] *= dobj_dfdg;
1884  adjm[1] = dobj_dg*matr[1]; matd[1] *= dobj_dfdg;
1885  adjm[2] = dobj_dg*matr[2]; matd[2] *= dobj_dfdg;
1886  adjm[3] = dobj_dg*matr[3]; matd[3] *= dobj_dfdg;
1887  adjm[4] = dobj_dg*matr[4]; matd[4] *= dobj_dfdg;
1888  adjm[5] = dobj_dg*matr[5]; matd[5] *= dobj_dfdg;
1889  adjm[6] = dobj_dg*matr[6]; matd[6] *= dobj_dfdg;
1890  adjm[7] = dobj_dg*matr[7]; matd[7] *= dobj_dfdg;
1891  adjm[8] = dobj_dg*matr[8]; matd[8] *= dobj_dfdg;
1892 
1893  /* Blocks for the Hessian construction */
1894  loc1 = dobj_dgdg*dg[0] + matd[0];
1895  J_A[0] = dobj_df + dg[0]*(matd[0] + loc1);
1896  J_A[1] = dg[0]*matd[1] + loc1*dg[1];
1897  J_A[2] = dg[0]*matd[2] + loc1*dg[2];
1898  J_B[0] = dg[0]*matd[3] + loc1*dg[3];
1899  J_B[1] = dg[0]*matd[4] + loc1*dg[4] + adjm[8];
1900  J_B[2] = dg[0]*matd[5] + loc1*dg[5] - adjm[7];
1901  J_C[0] = dg[0]*matd[6] + loc1*dg[6];
1902  J_C[1] = dg[0]*matd[7] + loc1*dg[7] - adjm[5];
1903  J_C[2] = dg[0]*matd[8] + loc1*dg[8] + adjm[4];
1904 
1905  loc1 = dobj_dgdg*dg[1] + matd[1];
1906  J_A[3] = dobj_df + dg[1]*(matd[1] + loc1);
1907  J_A[4] = dg[1]*matd[2] + loc1*dg[2];
1908  J_B[3] = dg[1]*matd[3] + loc1*dg[3] - adjm[8];
1909  J_B[4] = dg[1]*matd[4] + loc1*dg[4];
1910  J_B[5] = dg[1]*matd[5] + loc1*dg[5] + adjm[6];
1911  J_C[3] = dg[1]*matd[6] + loc1*dg[6] + adjm[5];
1912  J_C[4] = dg[1]*matd[7] + loc1*dg[7];
1913  J_C[5] = dg[1]*matd[8] + loc1*dg[8] - adjm[3];
1914 
1915  loc1 = dobj_dgdg*dg[2] + matd[2];
1916  J_A[5] = dobj_df + dg[2]*(matd[2] + loc1);
1917  J_B[6] = dg[2]*matd[3] + loc1*dg[3] + adjm[7];
1918  J_B[7] = dg[2]*matd[4] + loc1*dg[4] - adjm[6];
1919  J_B[8] = dg[2]*matd[5] + loc1*dg[5];
1920  J_C[6] = dg[2]*matd[6] + loc1*dg[6] - adjm[4];
1921  J_C[7] = dg[2]*matd[7] + loc1*dg[7] + adjm[3];
1922  J_C[8] = dg[2]*matd[8] + loc1*dg[8];
1923 
1924  loc1 = dobj_dgdg*dg[3] + matd[3];
1925  J_D[0] = dobj_df + dg[3]*(matd[3] + loc1);
1926  J_D[1] = dg[3]*matd[4] + loc1*dg[4];
1927  J_D[2] = dg[3]*matd[5] + loc1*dg[5];
1928  J_E[0] = dg[3]*matd[6] + loc1*dg[6];
1929  J_E[1] = dg[3]*matd[7] + loc1*dg[7] + adjm[2];
1930  J_E[2] = dg[3]*matd[8] + loc1*dg[8] - adjm[1];
1931 
1932  loc1 = dobj_dgdg*dg[4] + matd[4];
1933  J_D[3] = dobj_df + dg[4]*(matd[4] + loc1);
1934  J_D[4] = dg[4]*matd[5] + loc1*dg[5];
1935  J_E[3] = dg[4]*matd[6] + loc1*dg[6] - adjm[2];
1936  J_E[4] = dg[4]*matd[7] + loc1*dg[7];
1937  J_E[5] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
1938 
1939  loc1 = dobj_dgdg*dg[5] + matd[5];
1940  J_D[5] = dobj_df + dg[5]*(matd[5] + loc1);
1941  J_E[6] = dg[5]*matd[6] + loc1*dg[6] + adjm[1];
1942  J_E[7] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
1943  J_E[8] = dg[5]*matd[8] + loc1*dg[8];
1944 
1945  loc1 = dobj_dgdg*dg[6] + matd[6];
1946  J_F[0] = dobj_df + dg[6]*(matd[6] + loc1);
1947  J_F[1] = dg[6]*matd[7] + loc1*dg[7];
1948  J_F[2] = dg[6]*matd[8] + loc1*dg[8];
1949 
1950  loc1 = dobj_dgdg*dg[7] + matd[7];
1951  J_F[3] = dobj_df + dg[7]*(matd[7] + loc1);
1952  J_F[4] = dg[7]*matd[8] + loc1*dg[8];
1953 
1954  J_F[5] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
1955 
1956  /* Assemble matrix products */
1957 
1958  /* dx / dx */
1959  A[0] = J_A[0]*invR[0][0] + J_A[1]*invR[0][1] + J_A[2]*invR[0][2];
1960  A[1] = J_A[1]*invR[0][0] + J_A[3]*invR[0][1] + J_A[4]*invR[0][2];
1961  A[2] = J_A[2]*invR[0][0] + J_A[4]*invR[0][1] + J_A[5]*invR[0][2];
1962  h_obj[0][0] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
1963 
1964  /* dx / dy */
1965  A[0] = J_B[0]*invR[0][0] + J_B[1]*invR[0][1] + J_B[2]*invR[0][2];
1966  A[1] = J_B[3]*invR[0][0] + J_B[4]*invR[0][1] + J_B[5]*invR[0][2];
1967  A[2] = J_B[6]*invR[0][0] + J_B[7]*invR[0][1] + J_B[8]*invR[0][2];
1968  h_obj[0][1] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
1969 
1970  /* dx / dz */
1971  A[0] = J_C[0]*invR[0][0] + J_C[1]*invR[0][1] + J_C[2]*invR[0][2];
1972  A[1] = J_C[3]*invR[0][0] + J_C[4]*invR[0][1] + J_C[5]*invR[0][2];
1973  A[2] = J_C[6]*invR[0][0] + J_C[7]*invR[0][1] + J_C[8]*invR[0][2];
1974  h_obj[0][2] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
1975 
1976  /* dy / dy */
1977  A[0] = J_D[0]*invR[0][0] + J_D[1]*invR[0][1] + J_D[2]*invR[0][2];
1978  A[1] = J_D[1]*invR[0][0] + J_D[3]*invR[0][1] + J_D[4]*invR[0][2];
1979  A[2] = J_D[2]*invR[0][0] + J_D[4]*invR[0][1] + J_D[5]*invR[0][2];
1980  h_obj[1][1] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
1981 
1982  /* dy / dz */
1983  A[0] = J_E[0]*invR[0][0] + J_E[1]*invR[0][1] + J_E[2]*invR[0][2];
1984  A[1] = J_E[3]*invR[0][0] + J_E[4]*invR[0][1] + J_E[5]*invR[0][2];
1985  A[2] = J_E[6]*invR[0][0] + J_E[7]*invR[0][1] + J_E[8]*invR[0][2];
1986  h_obj[1][2] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
1987 
1988  /* dz / dz */
1989  A[0] = J_F[0]*invR[0][0] + J_F[1]*invR[0][1] + J_F[2]*invR[0][2];
1990  A[1] = J_F[1]*invR[0][0] + J_F[3]*invR[0][1] + J_F[4]*invR[0][2];
1991  A[2] = J_F[2]*invR[0][0] + J_F[4]*invR[0][1] + J_F[5]*invR[0][2];
1992  h_obj[2][2] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
1993 
1994  /* Complete diagonal blocks */
1995  h_obj.fill_lower_triangle();
1996  return true;
1997  }
1998 
1999  inline bool h_gdft_2_v2(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
2000  const Vector3D x[3], const Vector3D &n,
2001  const Matrix3D &invR, /* upper triangular */
2002  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
2003  const double alpha = 0.5,/* constant */
2004  const Exponent& gamma = 1.0,/* simplicial elements */
2005  const double delta = 0.0,/* max in denominator */
2006  const double beta = 0.0)/* no modification */
2007  {
2008  double matr[9], f, t1, t2;
2009  double matd[9], g, t3, loc1;
2010  double adjm[6], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
2011  double J_A[3], J_B[4], J_C[4], J_D[3], J_E[4], J_F[3];
2012  double A[2];
2013 
2014  /* Calculate M = A*inv(R). */
2015  f = x[1][0] - x[0][0];
2016  g = x[2][0] - x[0][0];
2017  matr[0] = f*invR[0][0];
2018  matr[1] = f*invR[0][1] + g*invR[1][1];
2019  matr[2] = f*invR[0][2] + g*invR[1][2] + n[0]*invR[2][2];
2020 
2021  f = x[1][1] - x[0][1];
2022  g = x[2][1] - x[0][1];
2023  matr[3] = f*invR[0][0];
2024  matr[4] = f*invR[0][1] + g*invR[1][1];
2025  matr[5] = f*invR[0][2] + g*invR[1][2] + n[1]*invR[2][2];
2026 
2027  f = x[1][2] - x[0][2];
2028  g = x[2][2] - x[0][2];
2029  matr[6] = f*invR[0][0];
2030  matr[7] = f*invR[0][1] + g*invR[1][1];
2031  matr[8] = f*invR[0][2] + g*invR[1][2] + n[2]*invR[2][2];
2032 
2033  /* Calculate det(M). */
2034  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
2035  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
2036  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
2037  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
2038  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
2039  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
2040  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
2041 
2042  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
2043 
2044  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
2045 
2046  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
2047  t2 = t1*t1 + 4.0*delta*delta;
2048  t3 = sqrt(t2);
2049  g = t1 + t3;
2050 
2051  /* Calculate N = M - beta*Q. */
2052  matd[0] = matr[0] - beta*Q[0][0];
2053  matd[1] = matr[1] - beta*Q[0][1];
2054  matd[2] = matr[2] - beta*Q[0][2];
2055  matd[3] = matr[3] - beta*Q[1][0];
2056  matd[4] = matr[4] - beta*Q[1][1];
2057  matd[5] = matr[5] - beta*Q[1][2];
2058  matd[6] = matr[6] - beta*Q[2][0];
2059  matd[7] = matr[7] - beta*Q[2][1];
2060  matd[8] = matr[8] - beta*Q[2][2];
2061 
2062  /* Calculate norm(N) */
2063  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
2064  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
2065  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
2066 
2067  /* Calculate objective function. */
2068  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
2069  obj = f * loc1;
2070 
2071  /* Calculate the derivative of the objective function. */
2072  t3 = 1.0 / t3;
2073  dobj_df = 2.0 * loc1;
2074  dobj_dg = -gamma * obj * t3;
2075  dobj_dfdg = -gamma * dobj_df * t3;
2076  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
2077 
2078  /* Calculate adjoint matrix */
2079  adjm[0] = dobj_df*matd[1] + dobj_dg*dg[1];
2080  adjm[1] = dobj_df*matd[2] + dobj_dg*dg[2];
2081  adjm[2] = dobj_df*matd[4] + dobj_dg*dg[4];
2082  adjm[3] = dobj_df*matd[5] + dobj_dg*dg[5];
2083  adjm[4] = dobj_df*matd[7] + dobj_dg*dg[7];
2084  adjm[5] = dobj_df*matd[8] + dobj_dg*dg[8];
2085 
2086  /* Construct gradients */
2087  g_obj[0] = invR[1][1]*adjm[0]+invR[1][2]*adjm[1];
2088  g_obj[1] = invR[1][1]*adjm[2]+invR[1][2]*adjm[3];
2089  g_obj[2] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
2090 
2091  /* Start of the Hessian evaluation */
2092  adjm[0] = dobj_dg*matr[0];
2093  adjm[1] = dobj_dg*matr[3];
2094  adjm[2] = dobj_dg*matr[6];
2095 
2096  matd[1] *= dobj_dfdg;
2097  matd[2] *= dobj_dfdg;
2098  matd[4] *= dobj_dfdg;
2099  matd[5] *= dobj_dfdg;
2100  matd[7] *= dobj_dfdg;
2101  matd[8] *= dobj_dfdg;
2102 
2103  /* Blocks for the Hessian construction */
2104  loc1 = dobj_dgdg*dg[1] + matd[1];
2105  J_A[0] = dobj_df + dg[1]*(matd[1] + loc1);
2106  J_A[1] = dg[1]*matd[2] + loc1*dg[2];
2107  J_B[0] = dg[1]*matd[4] + loc1*dg[4];
2108  J_B[1] = dg[1]*matd[5] + loc1*dg[5] + adjm[2];
2109  J_C[0] = dg[1]*matd[7] + loc1*dg[7];
2110  J_C[1] = dg[1]*matd[8] + loc1*dg[8] - adjm[1];
2111 
2112  loc1 = dobj_dgdg*dg[2] + matd[2];
2113  J_A[2] = dobj_df + dg[2]*(matd[2] + loc1);
2114  J_B[2] = dg[2]*matd[4] + loc1*dg[4] - adjm[2];
2115  J_B[3] = dg[2]*matd[5] + loc1*dg[5];
2116  J_C[2] = dg[2]*matd[7] + loc1*dg[7] + adjm[1];
2117  J_C[3] = dg[2]*matd[8] + loc1*dg[8];
2118 
2119  loc1 = dobj_dgdg*dg[4] + matd[4];
2120  J_D[0] = dobj_df + dg[4]*(matd[4] + loc1);
2121  J_D[1] = dg[4]*matd[5] + loc1*dg[5];
2122  J_E[0] = dg[4]*matd[7] + loc1*dg[7];
2123  J_E[1] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
2124 
2125  loc1 = dobj_dgdg*dg[5] + matd[5];
2126  J_D[2] = dobj_df + dg[5]*(matd[5] + loc1);
2127  J_E[2] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
2128  J_E[3] = dg[5]*matd[8] + loc1*dg[8];
2129 
2130  loc1 = dobj_dgdg*dg[7] + matd[7];
2131  J_F[0] = dobj_df + dg[7]*(matd[7] + loc1);
2132  J_F[1] = dg[7]*matd[8] + loc1*dg[8];
2133 
2134  J_F[2] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
2135 
2136  /* Assemble matrix products */
2137 
2138  /* dx / dx */
2139  A[0] = J_A[0]*invR[1][1] + J_A[1]*invR[1][2];
2140  A[1] = J_A[1]*invR[1][1] + J_A[2]*invR[1][2];
2141  h_obj[0][0] = A[0]*invR[1][1] + A[1]*invR[1][2];
2142 
2143  /* dx / dy */
2144  A[0] = J_B[0]*invR[1][1] + J_B[1]*invR[1][2];
2145  A[1] = J_B[2]*invR[1][1] + J_B[3]*invR[1][2];
2146  h_obj[0][1] = A[0]*invR[1][1] + A[1]*invR[1][2];
2147 
2148  /* dx / dz */
2149  A[0] = J_C[0]*invR[1][1] + J_C[1]*invR[1][2];
2150  A[1] = J_C[2]*invR[1][1] + J_C[3]*invR[1][2];
2151  h_obj[0][2] = A[0]*invR[1][1] + A[1]*invR[1][2];
2152 
2153  /* dy / dy */
2154  A[0] = J_D[0]*invR[1][1] + J_D[1]*invR[1][2];
2155  A[1] = J_D[1]*invR[1][1] + J_D[2]*invR[1][2];
2156  h_obj[1][1] = A[0]*invR[1][1] + A[1]*invR[1][2];
2157 
2158  /* dy / dz */
2159  A[0] = J_E[0]*invR[1][1] + J_E[1]*invR[1][2];
2160  A[1] = J_E[2]*invR[1][1] + J_E[3]*invR[1][2];
2161  h_obj[1][2] = A[0]*invR[1][1] + A[1]*invR[1][2];
2162 
2163  /* dz / dz */
2164  A[0] = J_F[0]*invR[1][1] + J_F[1]*invR[1][2];
2165  A[1] = J_F[1]*invR[1][1] + J_F[2]*invR[1][2];
2166  h_obj[2][2] = A[0]*invR[1][1] + A[1]*invR[1][2];
2167 
2168  /* Complete diagonal blocks */
2169  h_obj.fill_lower_triangle();
2170  return true;
2171  }
2172 
2173  inline bool g_gdft_3_v0(double &obj, Vector3D &g_obj,
2174  const Vector3D x[4],
2175  const Matrix3D &invR, /* upper triangular */
2176  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
2177  const double alpha = 1.0/3.0,/* constant */
2178  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
2179  const double delta = 0.0,/* max in denominator */
2180  const double beta = 0.0)/* no modification */
2181  {
2182  double matr[9], f, t1, t2;
2183  double matd[9], g;
2184  double adjm[9], loc1, loc2, loc3, loc4;
2185 
2186  /* Calculate M = A*inv(R). */
2187  f = x[1][0] - x[0][0];
2188  g = x[2][0] - x[0][0];
2189  t1 = x[3][0] - x[0][0];
2190  matr[0] = f*invR[0][0];
2191  matr[1] = f*invR[0][1] + g*invR[1][1];
2192  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2193 
2194  f = x[1][1] - x[0][1];
2195  g = x[2][1] - x[0][1];
2196  t1 = x[3][1] - x[0][1];
2197  matr[3] = f*invR[0][0];
2198  matr[4] = f*invR[0][1] + g*invR[1][1];
2199  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2200 
2201  f = x[1][2] - x[0][2];
2202  g = x[2][2] - x[0][2];
2203  t1 = x[3][2] - x[0][2];
2204  matr[6] = f*invR[0][0];
2205  matr[7] = f*invR[0][1] + g*invR[1][1];
2206  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2207 
2208  /* Calculate det(M). */
2209  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
2210  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
2211  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
2212  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
2213 
2214  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
2215 
2216  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
2217  t2 = sqrt(t1*t1 + 4.0*delta*delta);
2218  g = t1 + t2;
2219 
2220  /* Calculate N = M - beta*Q. */
2221  matd[0] = matr[0] - beta*Q[0][0];
2222  matd[1] = matr[1] - beta*Q[0][1];
2223  matd[2] = matr[2] - beta*Q[0][2];
2224  matd[3] = matr[3] - beta*Q[1][0];
2225  matd[4] = matr[4] - beta*Q[1][1];
2226  matd[5] = matr[5] - beta*Q[1][2];
2227  matd[6] = matr[6] - beta*Q[2][0];
2228  matd[7] = matr[7] - beta*Q[2][1];
2229  matd[8] = matr[8] - beta*Q[2][2];
2230 
2231  /* Calculate norm(N) */
2232  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
2233  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
2234  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
2235 
2236  /* Calculate objective function. */
2237  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
2238  obj = f * loc4;
2239 
2240  /* Calculate the derivative of the objective function. */
2241  f = 2.0 * loc4;
2242  g = -gamma * obj / t2;
2243 
2244  /* Calculate adjoint matrix */
2245  adjm[0] = f*matd[0] + g*loc1;
2246  adjm[1] = f*matd[1] + g*loc2;
2247  adjm[2] = f*matd[2] + g*loc3;
2248 
2249  loc1 = g*matr[0];
2250  loc2 = g*matr[1];
2251  loc3 = g*matr[2];
2252 
2253  adjm[3] = f*matd[3] + loc3*matr[7] - loc2*matr[8];
2254  adjm[4] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
2255  adjm[5] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
2256 
2257  adjm[6] = f*matd[6] + loc2*matr[5] - loc3*matr[4];
2258  adjm[7] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
2259  adjm[8] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
2260 
2261  /* Construct gradients */
2262  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
2263  g_obj[0] += invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
2264  g_obj[0] += invR[2][2]*adjm[2];
2265  g_obj[0] = -g_obj[0];
2266 
2267  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
2268  g_obj[1] += invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
2269  g_obj[1] += invR[2][2]*adjm[5];
2270  g_obj[1] = -g_obj[1];
2271 
2272  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
2273  g_obj[2] += invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
2274  g_obj[2] += invR[2][2]*adjm[8];
2275  g_obj[2] = -g_obj[2];
2276  return true;
2277  }
2278 
2279  inline bool g_gdft_3_v1(double &obj, Vector3D &g_obj,
2280  const Vector3D x[4],
2281  const Matrix3D &invR, /* upper triangular */
2282  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
2283  const double alpha = 1.0/3.0,/* constant */
2284  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
2285  const double delta = 0.0,/* max in denominator */
2286  const double beta = 0.0)/* no modification */
2287  {
2288  double matr[9], f, t1, t2;
2289  double matd[9], g;
2290  double adjm[9], loc1, loc2, loc3, loc4;
2291 
2292  /* Calculate M = A*inv(R). */
2293  f = x[1][0] - x[0][0];
2294  g = x[2][0] - x[0][0];
2295  t1 = x[3][0] - x[0][0];
2296  matr[0] = f*invR[0][0];
2297  matr[1] = f*invR[0][1] + g*invR[1][1];
2298  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2299 
2300  f = x[1][1] - x[0][1];
2301  g = x[2][1] - x[0][1];
2302  t1 = x[3][1] - x[0][1];
2303  matr[3] = f*invR[0][0];
2304  matr[4] = f*invR[0][1] + g*invR[1][1];
2305  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2306 
2307  f = x[1][2] - x[0][2];
2308  g = x[2][2] - x[0][2];
2309  t1 = x[3][2] - x[0][2];
2310  matr[6] = f*invR[0][0];
2311  matr[7] = f*invR[0][1] + g*invR[1][1];
2312  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2313 
2314  /* Calculate det(M). */
2315  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
2316  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
2317  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
2318  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
2319 
2320  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
2321 
2322  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
2323  t2 = sqrt(t1*t1 + 4.0*delta*delta);
2324  g = t1 + t2;
2325 
2326  /* Calculate N = M - beta*Q. */
2327  matd[0] = matr[0] - beta*Q[0][0];
2328  matd[1] = matr[1] - beta*Q[0][1];
2329  matd[2] = matr[2] - beta*Q[0][2];
2330  matd[3] = matr[3] - beta*Q[1][0];
2331  matd[4] = matr[4] - beta*Q[1][1];
2332  matd[5] = matr[5] - beta*Q[1][2];
2333  matd[6] = matr[6] - beta*Q[2][0];
2334  matd[7] = matr[7] - beta*Q[2][1];
2335  matd[8] = matr[8] - beta*Q[2][2];
2336 
2337  /* Calculate norm(N) */
2338  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
2339  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
2340  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
2341 
2342  /* Calculate objective function. */
2343  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
2344  obj = f * loc4;
2345 
2346  /* Calculate the derivative of the objective function. */
2347  f = 2.0 * loc4;
2348  g = -gamma * obj / t2;
2349 
2350  /* Calculate adjoint matrix */
2351  adjm[0] = f*matd[0] + g*loc1;
2352  adjm[1] = f*matd[1] + g*loc2;
2353  adjm[2] = f*matd[2] + g*loc3;
2354 
2355  loc1 = g*matr[0];
2356  loc2 = g*matr[1];
2357  loc3 = g*matr[2];
2358 
2359  adjm[3] = f*matd[3] + loc3*matr[7] - loc2*matr[8];
2360  adjm[4] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
2361  adjm[5] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
2362 
2363  adjm[6] = f*matd[6] + loc2*matr[5] - loc3*matr[4];
2364  adjm[7] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
2365  adjm[8] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
2366 
2367  /* Construct gradients */
2368  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
2369  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
2370  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
2371  return true;
2372  }
2373 
2374  inline bool g_gdft_3_v2(double &obj, Vector3D &g_obj,
2375  const Vector3D x[4],
2376  const Matrix3D &invR, /* upper triangular */
2377  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
2378  const double alpha = 1.0/3.0,/* constant */
2379  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
2380  const double delta = 0.0,/* max in denominator */
2381  const double beta = 0.0)/* no modification */
2382  {
2383  double matr[9], f, t1, t2;
2384  double matd[9], g;
2385  double adjm[6], loc1, loc2, loc3, loc4;
2386 
2387  /* Calculate M = A*inv(R). */
2388  f = x[1][0] - x[0][0];
2389  g = x[2][0] - x[0][0];
2390  t1 = x[3][0] - x[0][0];
2391  matr[0] = f*invR[0][0];
2392  matr[1] = f*invR[0][1] + g*invR[1][1];
2393  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2394 
2395  f = x[1][1] - x[0][1];
2396  g = x[2][1] - x[0][1];
2397  t1 = x[3][1] - x[0][1];
2398  matr[3] = f*invR[0][0];
2399  matr[4] = f*invR[0][1] + g*invR[1][1];
2400  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2401 
2402  f = x[1][2] - x[0][2];
2403  g = x[2][2] - x[0][2];
2404  t1 = x[3][2] - x[0][2];
2405  matr[6] = f*invR[0][0];
2406  matr[7] = f*invR[0][1] + g*invR[1][1];
2407  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2408 
2409  /* Calculate det(M). */
2410  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
2411  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
2412  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
2413  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
2414 
2415  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
2416 
2417  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
2418  t2 = sqrt(t1*t1 + 4.0*delta*delta);
2419  g = t1 + t2;
2420 
2421  /* Calculate N = M - beta*Q. */
2422  matd[0] = matr[0] - beta*Q[0][0];
2423  matd[1] = matr[1] - beta*Q[0][1];
2424  matd[2] = matr[2] - beta*Q[0][2];
2425  matd[3] = matr[3] - beta*Q[1][0];
2426  matd[4] = matr[4] - beta*Q[1][1];
2427  matd[5] = matr[5] - beta*Q[1][2];
2428  matd[6] = matr[6] - beta*Q[2][0];
2429  matd[7] = matr[7] - beta*Q[2][1];
2430  matd[8] = matr[8] - beta*Q[2][2];
2431 
2432  /* Calculate norm(N) */
2433  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
2434  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
2435  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
2436 
2437  /* Calculate objective function. */
2438  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
2439  obj = f * loc4;
2440 
2441  /* Calculate the derivative of the objective function. */
2442  f = 2.0 * loc4;
2443  g = -gamma * obj / t2;
2444 
2445  /* Calculate adjoint matrix */
2446  adjm[0] = f*matd[1] + g*loc2;
2447  adjm[1] = f*matd[2] + g*loc3;
2448 
2449  loc1 = g*matr[0];
2450  loc2 = g*matr[1];
2451  loc3 = g*matr[2];
2452 
2453  adjm[2] = f*matd[4] + loc1*matr[8] - loc3*matr[6];
2454  adjm[3] = f*matd[5] + loc2*matr[6] - loc1*matr[7];
2455 
2456  adjm[4] = f*matd[7] + loc3*matr[3] - loc1*matr[5];
2457  adjm[5] = f*matd[8] + loc1*matr[4] - loc2*matr[3];
2458 
2459  /* Construct gradients */
2460  g_obj[0] = invR[1][1]*adjm[0]+invR[1][2]*adjm[1];
2461  g_obj[1] = invR[1][1]*adjm[2]+invR[1][2]*adjm[3];
2462  g_obj[2] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
2463  return true;
2464  }
2465 
2466  inline bool g_gdft_3_v3(double &obj, Vector3D &g_obj,
2467  const Vector3D x[4],
2468  const Matrix3D &invR, /* upper triangular */
2469  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
2470  const double alpha = 1.0/3.0,/* constant */
2471  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
2472  const double delta = 0.0,/* max in denominator */
2473  const double beta = 0.0)/* no modification */
2474  {
2475  double matr[9], f, t1, t2;
2476  double matd[9], g;
2477  double loc1, loc2, loc3, loc4;
2478 
2479  /* Calculate M = A*inv(R). */
2480  f = x[1][0] - x[0][0];
2481  g = x[2][0] - x[0][0];
2482  t1 = x[3][0] - x[0][0];
2483  matr[0] = f*invR[0][0];
2484  matr[1] = f*invR[0][1] + g*invR[1][1];
2485  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2486 
2487  f = x[1][1] - x[0][1];
2488  g = x[2][1] - x[0][1];
2489  t1 = x[3][1] - x[0][1];
2490  matr[3] = f*invR[0][0];
2491  matr[4] = f*invR[0][1] + g*invR[1][1];
2492  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2493 
2494  f = x[1][2] - x[0][2];
2495  g = x[2][2] - x[0][2];
2496  t1 = x[3][2] - x[0][2];
2497  matr[6] = f*invR[0][0];
2498  matr[7] = f*invR[0][1] + g*invR[1][1];
2499  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2500 
2501  /* Calculate det(M). */
2502  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
2503  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
2504  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
2505  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
2506 
2507  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
2508 
2509  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
2510  t2 = sqrt(t1*t1 + 4.0*delta*delta);
2511  g = t1 + t2;
2512 
2513  /* Calculate N = M - beta*Q. */
2514  matd[0] = matr[0] - beta*Q[0][0];
2515  matd[1] = matr[1] - beta*Q[0][1];
2516  matd[2] = matr[2] - beta*Q[0][2];
2517  matd[3] = matr[3] - beta*Q[1][0];
2518  matd[4] = matr[4] - beta*Q[1][1];
2519  matd[5] = matr[5] - beta*Q[1][2];
2520  matd[6] = matr[6] - beta*Q[2][0];
2521  matd[7] = matr[7] - beta*Q[2][1];
2522  matd[8] = matr[8] - beta*Q[2][2];
2523 
2524  /* Calculate norm(N) */
2525  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
2526  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
2527  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
2528 
2529  /* Calculate objective function. */
2530  loc4 = alpha * pow(2.0, gamma) / pow(g, gamma);
2531  obj = f * loc4;
2532 
2533  /* Calculate the derivative of the objective function. */
2534  f = 2.0 * loc4;
2535  g = -gamma * obj / t2;
2536 
2537  /* Construct gradients */
2538  loc1 = g*matr[0];
2539  loc2 = g*matr[1];
2540 
2541  g_obj[0] = invR[2][2]*(f*matd[2] + g*loc3);
2542  g_obj[1] = invR[2][2]*(f*matd[5] + loc2*matr[6] - loc1*matr[7]);
2543  g_obj[2] = invR[2][2]*(f*matd[8] + loc1*matr[4] - loc2*matr[3]);
2544  return true;
2545  }
2546 
2547  inline bool h_gdft_3_v0(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
2548  const Vector3D x[4],
2549  const Matrix3D &invR, /* upper triangular */
2550  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
2551  const double alpha = 1.0/3.0,/* constant */
2552  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
2553  const double delta = 0.0,/* max in denominator */
2554  const double beta = 0.0)/* no modification */
2555  {
2556  double matr[9], f, t1, t2;
2557  double matd[9], g, t3, loc1;
2558  double adjm[9], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
2559  double J_A[6], J_B[10], J_C[10], J_D[6], J_E[10], J_F[6];
2560  double A[12];
2561 
2562  /* Calculate M = A*inv(R). */
2563  f = x[1][0] - x[0][0];
2564  g = x[2][0] - x[0][0];
2565  t1 = x[3][0] - x[0][0];
2566  matr[0] = f*invR[0][0];
2567  matr[1] = f*invR[0][1] + g*invR[1][1];
2568  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2569 
2570  f = x[1][1] - x[0][1];
2571  g = x[2][1] - x[0][1];
2572  t1 = x[3][1] - x[0][1];
2573  matr[3] = f*invR[0][0];
2574  matr[4] = f*invR[0][1] + g*invR[1][1];
2575  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2576 
2577  f = x[1][2] - x[0][2];
2578  g = x[2][2] - x[0][2];
2579  t1 = x[3][2] - x[0][2];
2580  matr[6] = f*invR[0][0];
2581  matr[7] = f*invR[0][1] + g*invR[1][1];
2582  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2583 
2584  /* Calculate det(M). */
2585  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
2586  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
2587  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
2588  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
2589  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
2590  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
2591  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
2592  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
2593  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
2594 
2595  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
2596 
2597  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
2598 
2599  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
2600  t2 = t1*t1 + 4.0*delta*delta;
2601  t3 = sqrt(t2);
2602  g = t1 + t3;
2603 
2604  /* Calculate N = M - beta*Q. */
2605  matd[0] = matr[0] - beta*Q[0][0];
2606  matd[1] = matr[1] - beta*Q[0][1];
2607  matd[2] = matr[2] - beta*Q[0][2];
2608  matd[3] = matr[3] - beta*Q[1][0];
2609  matd[4] = matr[4] - beta*Q[1][1];
2610  matd[5] = matr[5] - beta*Q[1][2];
2611  matd[6] = matr[6] - beta*Q[2][0];
2612  matd[7] = matr[7] - beta*Q[2][1];
2613  matd[8] = matr[8] - beta*Q[2][2];
2614 
2615  /* Calculate norm(N) */
2616  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
2617  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
2618  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
2619 
2620  /* Calculate objective function. */
2621  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
2622  obj = f * loc1;
2623 
2624  /* Calculate the derivative of the objective function. */
2625  t3 = 1.0 / t3;
2626  dobj_df = 2.0 * loc1;
2627  dobj_dg = -gamma * obj * t3;
2628  dobj_dfdg = -gamma * dobj_df * t3;
2629  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
2630 
2631  /* Calculate adjoint matrix */
2632  adjm[0] = dobj_df*matd[0] + dobj_dg*dg[0];
2633  adjm[1] = dobj_df*matd[1] + dobj_dg*dg[1];
2634  adjm[2] = dobj_df*matd[2] + dobj_dg*dg[2];
2635  adjm[3] = dobj_df*matd[3] + dobj_dg*dg[3];
2636  adjm[4] = dobj_df*matd[4] + dobj_dg*dg[4];
2637  adjm[5] = dobj_df*matd[5] + dobj_dg*dg[5];
2638  adjm[6] = dobj_df*matd[6] + dobj_dg*dg[6];
2639  adjm[7] = dobj_df*matd[7] + dobj_dg*dg[7];
2640  adjm[8] = dobj_df*matd[8] + dobj_dg*dg[8];
2641 
2642  /* Construct gradients */
2643  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
2644  g_obj[0] += invR[1][1]*adjm[1]+invR[1][2]*adjm[2];
2645  g_obj[0] += invR[2][2]*adjm[2];
2646  g_obj[0] = -g_obj[0];
2647 
2648  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
2649  g_obj[1] += invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
2650  g_obj[1] += invR[2][2]*adjm[5];
2651  g_obj[1] = -g_obj[1];
2652 
2653  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
2654  g_obj[2] += invR[1][1]*adjm[7]+invR[1][2]*adjm[8];
2655  g_obj[2] += invR[2][2]*adjm[8];
2656  g_obj[2] = -g_obj[2];
2657 
2658  /* Start of the Hessian evaluation */
2659  adjm[0] = dobj_dg*matr[0]; matd[0] *= dobj_dfdg;
2660  adjm[1] = dobj_dg*matr[1]; matd[1] *= dobj_dfdg;
2661  adjm[2] = dobj_dg*matr[2]; matd[2] *= dobj_dfdg;
2662  adjm[3] = dobj_dg*matr[3]; matd[3] *= dobj_dfdg;
2663  adjm[4] = dobj_dg*matr[4]; matd[4] *= dobj_dfdg;
2664  adjm[5] = dobj_dg*matr[5]; matd[5] *= dobj_dfdg;
2665  adjm[6] = dobj_dg*matr[6]; matd[6] *= dobj_dfdg;
2666  adjm[7] = dobj_dg*matr[7]; matd[7] *= dobj_dfdg;
2667  adjm[8] = dobj_dg*matr[8]; matd[8] *= dobj_dfdg;
2668 
2669  /* Blocks for the Hessian construction */
2670  loc1 = dobj_dgdg*dg[0] + matd[0];
2671  J_A[0] = dobj_df + dg[0]*(matd[0] + loc1);
2672  J_A[1] = dg[0]*matd[1] + loc1*dg[1];
2673  J_A[2] = dg[0]*matd[2] + loc1*dg[2];
2674  J_B[0] = dg[0]*matd[3] + loc1*dg[3];
2675  J_B[1] = dg[0]*matd[4] + loc1*dg[4] + adjm[8];
2676  J_B[2] = dg[0]*matd[5] + loc1*dg[5] - adjm[7];
2677  J_C[0] = dg[0]*matd[6] + loc1*dg[6];
2678  J_C[1] = dg[0]*matd[7] + loc1*dg[7] - adjm[5];
2679  J_C[2] = dg[0]*matd[8] + loc1*dg[8] + adjm[4];
2680 
2681  loc1 = dobj_dgdg*dg[1] + matd[1];
2682  J_A[3] = dobj_df + dg[1]*(matd[1] + loc1);
2683  J_A[4] = dg[1]*matd[2] + loc1*dg[2];
2684  J_B[3] = dg[1]*matd[3] + loc1*dg[3] - adjm[8];
2685  J_B[4] = dg[1]*matd[4] + loc1*dg[4];
2686  J_B[5] = dg[1]*matd[5] + loc1*dg[5] + adjm[6];
2687  J_C[3] = dg[1]*matd[6] + loc1*dg[6] + adjm[5];
2688  J_C[4] = dg[1]*matd[7] + loc1*dg[7];
2689  J_C[5] = dg[1]*matd[8] + loc1*dg[8] - adjm[3];
2690 
2691  loc1 = dobj_dgdg*dg[2] + matd[2];
2692  J_A[5] = dobj_df + dg[2]*(matd[2] + loc1);
2693  J_B[6] = dg[2]*matd[3] + loc1*dg[3] + adjm[7];
2694  J_B[7] = dg[2]*matd[4] + loc1*dg[4] - adjm[6];
2695  J_B[8] = dg[2]*matd[5] + loc1*dg[5];
2696  J_C[6] = dg[2]*matd[6] + loc1*dg[6] - adjm[4];
2697  J_C[7] = dg[2]*matd[7] + loc1*dg[7] + adjm[3];
2698  J_C[8] = dg[2]*matd[8] + loc1*dg[8];
2699 
2700  loc1 = dobj_dgdg*dg[3] + matd[3];
2701  J_D[0] = dobj_df + dg[3]*(matd[3] + loc1);
2702  J_D[1] = dg[3]*matd[4] + loc1*dg[4];
2703  J_D[2] = dg[3]*matd[5] + loc1*dg[5];
2704  J_E[0] = dg[3]*matd[6] + loc1*dg[6];
2705  J_E[1] = dg[3]*matd[7] + loc1*dg[7] + adjm[2];
2706  J_E[2] = dg[3]*matd[8] + loc1*dg[8] - adjm[1];
2707 
2708  loc1 = dobj_dgdg*dg[4] + matd[4];
2709  J_D[3] = dobj_df + dg[4]*(matd[4] + loc1);
2710  J_D[4] = dg[4]*matd[5] + loc1*dg[5];
2711  J_E[3] = dg[4]*matd[6] + loc1*dg[6] - adjm[2];
2712  J_E[4] = dg[4]*matd[7] + loc1*dg[7];
2713  J_E[5] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
2714 
2715  loc1 = dobj_dgdg*dg[5] + matd[5];
2716  J_D[5] = dobj_df + dg[5]*(matd[5] + loc1);
2717  J_E[6] = dg[5]*matd[6] + loc1*dg[6] + adjm[1];
2718  J_E[7] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
2719  J_E[8] = dg[5]*matd[8] + loc1*dg[8];
2720 
2721  loc1 = dobj_dgdg*dg[6] + matd[6];
2722  J_F[0] = dobj_df + dg[6]*(matd[6] + loc1);
2723  J_F[1] = dg[6]*matd[7] + loc1*dg[7];
2724  J_F[2] = dg[6]*matd[8] + loc1*dg[8];
2725 
2726  loc1 = dobj_dgdg*dg[7] + matd[7];
2727  J_F[3] = dobj_df + dg[7]*(matd[7] + loc1);
2728  J_F[4] = dg[7]*matd[8] + loc1*dg[8];
2729 
2730  J_F[5] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
2731 
2732  /* Assemble matrix products */
2733 
2734  /* dx / dx */
2735  A[1] = J_A[0]*invR[0][0] + J_A[1]*invR[0][1] + J_A[2]*invR[0][2];
2736  A[2] = J_A[1]*invR[1][1] + J_A[2]*invR[1][2];
2737  A[3] = J_A[2]*invR[2][2];
2738  A[0] = -A[1] - A[2] - A[3];
2739 
2740  A[5] = J_A[1]*invR[0][0] + J_A[3]*invR[0][1] + J_A[4]*invR[0][2];
2741  A[6] = J_A[3]*invR[1][1] + J_A[4]*invR[1][2];
2742  A[7] = J_A[4]*invR[2][2];
2743  A[4] = -A[5] - A[6] - A[7];
2744 
2745  A[9] = J_A[2]*invR[0][0] + J_A[4]*invR[0][1] + J_A[5]*invR[0][2];
2746  A[10] = J_A[4]*invR[1][1] + J_A[5]*invR[1][2];
2747  A[11] = J_A[5]*invR[2][2];
2748  A[8] = -A[9] - A[10] - A[11];
2749 
2750  h_obj[0][0] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
2751  h_obj[0][0] += A[4]*invR[1][1] + A[8]*invR[1][2];
2752  h_obj[0][0] += A[8]*invR[2][2];
2753  h_obj[0][0] = -h_obj[0][0];
2754 
2755  /* dx / dy */
2756  A[1] = J_B[0]*invR[0][0] + J_B[1]*invR[0][1] + J_B[2]*invR[0][2];
2757  A[2] = J_B[1]*invR[1][1] + J_B[2]*invR[1][2];
2758  A[3] = J_B[2]*invR[2][2];
2759  A[0] = -A[1] - A[2] - A[3];
2760 
2761  A[5] = J_B[3]*invR[0][0] + J_B[4]*invR[0][1] + J_B[5]*invR[0][2];
2762  A[6] = J_B[4]*invR[1][1] + J_B[5]*invR[1][2];
2763  A[7] = J_B[5]*invR[2][2];
2764  A[4] = -A[5] - A[6] - A[7];
2765 
2766  A[9] = J_B[6]*invR[0][0] + J_B[7]*invR[0][1] + J_B[8]*invR[0][2];
2767  A[10] = J_B[7]*invR[1][1] + J_B[8]*invR[1][2];
2768  A[11] = J_B[8]*invR[2][2];
2769  A[8] = -A[9] - A[10] - A[11];
2770 
2771  h_obj[0][1] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
2772  h_obj[0][1] += A[4]*invR[1][1] + A[8]*invR[1][2];
2773  h_obj[0][1] += A[8]*invR[2][2];
2774  h_obj[0][1] = -h_obj[0][1];
2775 
2776  /* dx / dz */
2777  A[1] = J_C[0]*invR[0][0] + J_C[1]*invR[0][1] + J_C[2]*invR[0][2];
2778  A[2] = J_C[1]*invR[1][1] + J_C[2]*invR[1][2];
2779  A[3] = J_C[2]*invR[2][2];
2780  A[0] = -A[1] - A[2] - A[3];
2781 
2782  A[5] = J_C[3]*invR[0][0] + J_C[4]*invR[0][1] + J_C[5]*invR[0][2];
2783  A[6] = J_C[4]*invR[1][1] + J_C[5]*invR[1][2];
2784  A[7] = J_C[5]*invR[2][2];
2785  A[4] = -A[5] - A[6] - A[7];
2786 
2787  A[9] = J_C[6]*invR[0][0] + J_C[7]*invR[0][1] + J_C[8]*invR[0][2];
2788  A[10] = J_C[7]*invR[1][1] + J_C[8]*invR[1][2];
2789  A[11] = J_C[8]*invR[2][2];
2790  A[8] = -A[9] - A[10] - A[11];
2791 
2792  h_obj[0][2] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
2793  h_obj[0][2] += A[4]*invR[1][1] + A[8]*invR[1][2];
2794  h_obj[0][2] += A[8]*invR[2][2];
2795  h_obj[0][2] = -h_obj[0][2];
2796 
2797  /* dy / dy */
2798  A[1] = J_D[0]*invR[0][0] + J_D[1]*invR[0][1] + J_D[2]*invR[0][2];
2799  A[2] = J_D[1]*invR[1][1] + J_D[2]*invR[1][2];
2800  A[3] = J_D[2]*invR[2][2];
2801  A[0] = -A[1] - A[2] - A[3];
2802 
2803  A[5] = J_D[1]*invR[0][0] + J_D[3]*invR[0][1] + J_D[4]*invR[0][2];
2804  A[6] = J_D[3]*invR[1][1] + J_D[4]*invR[1][2];
2805  A[7] = J_D[4]*invR[2][2];
2806  A[4] = -A[5] - A[6] - A[7];
2807 
2808  A[9] = J_D[2]*invR[0][0] + J_D[4]*invR[0][1] + J_D[5]*invR[0][2];
2809  A[10] = J_D[4]*invR[1][1] + J_D[5]*invR[1][2];
2810  A[11] = J_D[5]*invR[2][2];
2811  A[8] = -A[9] - A[10] - A[11];
2812 
2813  h_obj[1][1] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
2814  h_obj[1][1] += A[4]*invR[1][1] + A[8]*invR[1][2];
2815  h_obj[1][1] += A[8]*invR[2][2];
2816  h_obj[1][1] = -h_obj[1][1];
2817 
2818  /* dy / dz */
2819  A[1] = J_E[0]*invR[0][0] + J_E[1]*invR[0][1] + J_E[2]*invR[0][2];
2820  A[2] = J_E[1]*invR[1][1] + J_E[2]*invR[1][2];
2821  A[3] = J_E[2]*invR[2][2];
2822  A[0] = -A[1] - A[2] - A[3];
2823 
2824  A[5] = J_E[3]*invR[0][0] + J_E[4]*invR[0][1] + J_E[5]*invR[0][2];
2825  A[6] = J_E[4]*invR[1][1] + J_E[5]*invR[1][2];
2826  A[7] = J_E[5]*invR[2][2];
2827  A[4] = -A[5] - A[6] - A[7];
2828 
2829  A[9] = J_E[6]*invR[0][0] + J_E[7]*invR[0][1] + J_E[8]*invR[0][2];
2830  A[10] = J_E[7]*invR[1][1] + J_E[8]*invR[1][2];
2831  A[11] = J_E[8]*invR[2][2];
2832  A[8] = -A[9] - A[10] - A[11];
2833 
2834  h_obj[1][2] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
2835  h_obj[1][2] += A[4]*invR[1][1] + A[8]*invR[1][2];
2836  h_obj[1][2] += A[8]*invR[2][2];
2837  h_obj[1][2] = -h_obj[1][2];
2838 
2839  /* dz / dz */
2840  A[1] = J_F[0]*invR[0][0] + J_F[1]*invR[0][1] + J_F[2]*invR[0][2];
2841  A[2] = J_F[1]*invR[1][1] + J_F[2]*invR[1][2];
2842  A[3] = J_F[2]*invR[2][2];
2843  A[0] = -A[1] - A[2] - A[3];
2844 
2845  A[5] = J_F[1]*invR[0][0] + J_F[3]*invR[0][1] + J_F[4]*invR[0][2];
2846  A[6] = J_F[3]*invR[1][1] + J_F[4]*invR[1][2];
2847  A[7] = J_F[4]*invR[2][2];
2848  A[4] = -A[5] - A[6] - A[7];
2849 
2850  A[9] = J_F[2]*invR[0][0] + J_F[4]*invR[0][1] + J_F[5]*invR[0][2];
2851  A[10] = J_F[4]*invR[1][1] + J_F[5]*invR[1][2];
2852  A[11] = J_F[5]*invR[2][2];
2853  A[8] = -A[9] - A[10] - A[11];
2854 
2855  h_obj[2][2] = A[0]*invR[0][0] + A[4]*invR[0][1] + A[8]*invR[0][2];
2856  h_obj[2][2] += A[4]*invR[1][1] + A[8]*invR[1][2];
2857  h_obj[2][2] += A[8]*invR[2][2];
2858  h_obj[2][2] = -h_obj[2][2];
2859 
2860  /* Complete diagonal blocks */
2861  h_obj.fill_lower_triangle();
2862  return true;
2863  }
2864 
2865  inline bool h_gdft_3_v1(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
2866  const Vector3D x[4],
2867  const Matrix3D &invR, /* upper triangular */
2868  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
2869  const double alpha = 1.0/3.0,/* constant */
2870  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
2871  const double delta = 0.0,/* max in denominator */
2872  const double beta = 0.0)/* no modification */
2873  {
2874  double matr[9], f, t1, t2;
2875  double matd[9], g, t3, loc1;
2876  double adjm[9], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
2877  double J_A[6], J_B[10], J_C[10], J_D[6], J_E[10], J_F[6];
2878  double A[12];
2879 
2880  /* Calculate M = A*inv(R). */
2881  f = x[1][0] - x[0][0];
2882  g = x[2][0] - x[0][0];
2883  t1 = x[3][0] - x[0][0];
2884  matr[0] = f*invR[0][0];
2885  matr[1] = f*invR[0][1] + g*invR[1][1];
2886  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2887 
2888  f = x[1][1] - x[0][1];
2889  g = x[2][1] - x[0][1];
2890  t1 = x[3][1] - x[0][1];
2891  matr[3] = f*invR[0][0];
2892  matr[4] = f*invR[0][1] + g*invR[1][1];
2893  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2894 
2895  f = x[1][2] - x[0][2];
2896  g = x[2][2] - x[0][2];
2897  t1 = x[3][2] - x[0][2];
2898  matr[6] = f*invR[0][0];
2899  matr[7] = f*invR[0][1] + g*invR[1][1];
2900  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
2901 
2902  /* Calculate det(M). */
2903  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
2904  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
2905  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
2906  dg[3] = matr[2]*matr[7] - matr[1]*matr[8];
2907  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
2908  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
2909  dg[6] = matr[1]*matr[5] - matr[2]*matr[4];
2910  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
2911  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
2912 
2913  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
2914 
2915  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
2916 
2917  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
2918  t2 = t1*t1 + 4.0*delta*delta;
2919  t3 = sqrt(t2);
2920  g = t1 + t3;
2921 
2922  /* Calculate N = M - beta*Q. */
2923  matd[0] = matr[0] - beta*Q[0][0];
2924  matd[1] = matr[1] - beta*Q[0][1];
2925  matd[2] = matr[2] - beta*Q[0][2];
2926  matd[3] = matr[3] - beta*Q[1][0];
2927  matd[4] = matr[4] - beta*Q[1][1];
2928  matd[5] = matr[5] - beta*Q[1][2];
2929  matd[6] = matr[6] - beta*Q[2][0];
2930  matd[7] = matr[7] - beta*Q[2][1];
2931  matd[8] = matr[8] - beta*Q[2][2];
2932 
2933  /* Calculate norm(N) */
2934  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
2935  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
2936  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
2937 
2938  /* Calculate objective function. */
2939  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
2940  obj = f * loc1;
2941 
2942  /* Calculate the derivative of the objective function. */
2943  t3 = 1.0 / t3;
2944  dobj_df = 2.0 * loc1;
2945  dobj_dg = -gamma * obj * t3;
2946  dobj_dfdg = -gamma * dobj_df * t3;
2947  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
2948 
2949  /* Calculate adjoint matrix */
2950  adjm[0] = dobj_df*matd[0] + dobj_dg*dg[0];
2951  adjm[1] = dobj_df*matd[1] + dobj_dg*dg[1];
2952  adjm[2] = dobj_df*matd[2] + dobj_dg*dg[2];
2953  adjm[3] = dobj_df*matd[3] + dobj_dg*dg[3];
2954  adjm[4] = dobj_df*matd[4] + dobj_dg*dg[4];
2955  adjm[5] = dobj_df*matd[5] + dobj_dg*dg[5];
2956  adjm[6] = dobj_df*matd[6] + dobj_dg*dg[6];
2957  adjm[7] = dobj_df*matd[7] + dobj_dg*dg[7];
2958  adjm[8] = dobj_df*matd[8] + dobj_dg*dg[8];
2959 
2960  /* Construct gradients */
2961  g_obj[0] = invR[0][0]*adjm[0]+invR[0][1]*adjm[1]+invR[0][2]*adjm[2];
2962  g_obj[1] = invR[0][0]*adjm[3]+invR[0][1]*adjm[4]+invR[0][2]*adjm[5];
2963  g_obj[2] = invR[0][0]*adjm[6]+invR[0][1]*adjm[7]+invR[0][2]*adjm[8];
2964 
2965  /* Start of the Hessian evaluation */
2966  adjm[0] = dobj_dg*matr[0]; matd[0] *= dobj_dfdg;
2967  adjm[1] = dobj_dg*matr[1]; matd[1] *= dobj_dfdg;
2968  adjm[2] = dobj_dg*matr[2]; matd[2] *= dobj_dfdg;
2969  adjm[3] = dobj_dg*matr[3]; matd[3] *= dobj_dfdg;
2970  adjm[4] = dobj_dg*matr[4]; matd[4] *= dobj_dfdg;
2971  adjm[5] = dobj_dg*matr[5]; matd[5] *= dobj_dfdg;
2972  adjm[6] = dobj_dg*matr[6]; matd[6] *= dobj_dfdg;
2973  adjm[7] = dobj_dg*matr[7]; matd[7] *= dobj_dfdg;
2974  adjm[8] = dobj_dg*matr[8]; matd[8] *= dobj_dfdg;
2975 
2976  /* Blocks for the Hessian construction */
2977  loc1 = dobj_dgdg*dg[0] + matd[0];
2978  J_A[0] = dobj_df + dg[0]*(matd[0] + loc1);
2979  J_A[1] = dg[0]*matd[1] + loc1*dg[1];
2980  J_A[2] = dg[0]*matd[2] + loc1*dg[2];
2981  J_B[0] = dg[0]*matd[3] + loc1*dg[3];
2982  J_B[1] = dg[0]*matd[4] + loc1*dg[4] + adjm[8];
2983  J_B[2] = dg[0]*matd[5] + loc1*dg[5] - adjm[7];
2984  J_C[0] = dg[0]*matd[6] + loc1*dg[6];
2985  J_C[1] = dg[0]*matd[7] + loc1*dg[7] - adjm[5];
2986  J_C[2] = dg[0]*matd[8] + loc1*dg[8] + adjm[4];
2987 
2988  loc1 = dobj_dgdg*dg[1] + matd[1];
2989  J_A[3] = dobj_df + dg[1]*(matd[1] + loc1);
2990  J_A[4] = dg[1]*matd[2] + loc1*dg[2];
2991  J_B[3] = dg[1]*matd[3] + loc1*dg[3] - adjm[8];
2992  J_B[4] = dg[1]*matd[4] + loc1*dg[4];
2993  J_B[5] = dg[1]*matd[5] + loc1*dg[5] + adjm[6];
2994  J_C[3] = dg[1]*matd[6] + loc1*dg[6] + adjm[5];
2995  J_C[4] = dg[1]*matd[7] + loc1*dg[7];
2996  J_C[5] = dg[1]*matd[8] + loc1*dg[8] - adjm[3];
2997 
2998  loc1 = dobj_dgdg*dg[2] + matd[2];
2999  J_A[5] = dobj_df + dg[2]*(matd[2] + loc1);
3000  J_B[6] = dg[2]*matd[3] + loc1*dg[3] + adjm[7];
3001  J_B[7] = dg[2]*matd[4] + loc1*dg[4] - adjm[6];
3002  J_B[8] = dg[2]*matd[5] + loc1*dg[5];
3003  J_C[6] = dg[2]*matd[6] + loc1*dg[6] - adjm[4];
3004  J_C[7] = dg[2]*matd[7] + loc1*dg[7] + adjm[3];
3005  J_C[8] = dg[2]*matd[8] + loc1*dg[8];
3006 
3007  loc1 = dobj_dgdg*dg[3] + matd[3];
3008  J_D[0] = dobj_df + dg[3]*(matd[3] + loc1);
3009  J_D[1] = dg[3]*matd[4] + loc1*dg[4];
3010  J_D[2] = dg[3]*matd[5] + loc1*dg[5];
3011  J_E[0] = dg[3]*matd[6] + loc1*dg[6];
3012  J_E[1] = dg[3]*matd[7] + loc1*dg[7] + adjm[2];
3013  J_E[2] = dg[3]*matd[8] + loc1*dg[8] - adjm[1];
3014 
3015  loc1 = dobj_dgdg*dg[4] + matd[4];
3016  J_D[3] = dobj_df + dg[4]*(matd[4] + loc1);
3017  J_D[4] = dg[4]*matd[5] + loc1*dg[5];
3018  J_E[3] = dg[4]*matd[6] + loc1*dg[6] - adjm[2];
3019  J_E[4] = dg[4]*matd[7] + loc1*dg[7];
3020  J_E[5] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
3021 
3022  loc1 = dobj_dgdg*dg[5] + matd[5];
3023  J_D[5] = dobj_df + dg[5]*(matd[5] + loc1);
3024  J_E[6] = dg[5]*matd[6] + loc1*dg[6] + adjm[1];
3025  J_E[7] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
3026  J_E[8] = dg[5]*matd[8] + loc1*dg[8];
3027 
3028  loc1 = dobj_dgdg*dg[6] + matd[6];
3029  J_F[0] = dobj_df + dg[6]*(matd[6] + loc1);
3030  J_F[1] = dg[6]*matd[7] + loc1*dg[7];
3031  J_F[2] = dg[6]*matd[8] + loc1*dg[8];
3032 
3033  loc1 = dobj_dgdg*dg[7] + matd[7];
3034  J_F[3] = dobj_df + dg[7]*(matd[7] + loc1);
3035  J_F[4] = dg[7]*matd[8] + loc1*dg[8];
3036 
3037  J_F[5] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
3038 
3039  /* Assemble matrix products */
3040 
3041  /* dx / dx */
3042  A[0] = J_A[0]*invR[0][0] + J_A[1]*invR[0][1] + J_A[2]*invR[0][2];
3043  A[1] = J_A[1]*invR[0][0] + J_A[3]*invR[0][1] + J_A[4]*invR[0][2];
3044  A[2] = J_A[2]*invR[0][0] + J_A[4]*invR[0][1] + J_A[5]*invR[0][2];
3045  h_obj[0][0] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
3046 
3047  /* dx / dy */
3048  A[0] = J_B[0]*invR[0][0] + J_B[1]*invR[0][1] + J_B[2]*invR[0][2];
3049  A[1] = J_B[3]*invR[0][0] + J_B[4]*invR[0][1] + J_B[5]*invR[0][2];
3050  A[2] = J_B[6]*invR[0][0] + J_B[7]*invR[0][1] + J_B[8]*invR[0][2];
3051  h_obj[0][1] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
3052 
3053  /* dx / dz */
3054  A[0] = J_C[0]*invR[0][0] + J_C[1]*invR[0][1] + J_C[2]*invR[0][2];
3055  A[1] = J_C[3]*invR[0][0] + J_C[4]*invR[0][1] + J_C[5]*invR[0][2];
3056  A[2] = J_C[6]*invR[0][0] + J_C[7]*invR[0][1] + J_C[8]*invR[0][2];
3057  h_obj[0][2] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
3058 
3059  /* dy / dy */
3060  A[0] = J_D[0]*invR[0][0] + J_D[1]*invR[0][1] + J_D[2]*invR[0][2];
3061  A[1] = J_D[1]*invR[0][0] + J_D[3]*invR[0][1] + J_D[4]*invR[0][2];
3062  A[2] = J_D[2]*invR[0][0] + J_D[4]*invR[0][1] + J_D[5]*invR[0][2];
3063  h_obj[1][1] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
3064 
3065  /* dy / dz */
3066  A[0] = J_E[0]*invR[0][0] + J_E[1]*invR[0][1] + J_E[2]*invR[0][2];
3067  A[1] = J_E[3]*invR[0][0] + J_E[4]*invR[0][1] + J_E[5]*invR[0][2];
3068  A[2] = J_E[6]*invR[0][0] + J_E[7]*invR[0][1] + J_E[8]*invR[0][2];
3069  h_obj[1][2] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
3070 
3071  /* dz / dz */
3072  A[0] = J_F[0]*invR[0][0] + J_F[1]*invR[0][1] + J_F[2]*invR[0][2];
3073  A[1] = J_F[1]*invR[0][0] + J_F[3]*invR[0][1] + J_F[4]*invR[0][2];
3074  A[2] = J_F[2]*invR[0][0] + J_F[4]*invR[0][1] + J_F[5]*invR[0][2];
3075  h_obj[2][2] = A[0]*invR[0][0] + A[1]*invR[0][1] + A[2]*invR[0][2];
3076 
3077  /* Complete diagonal blocks */
3078  h_obj.fill_lower_triangle();
3079  return true;
3080  }
3081 
3082  inline bool h_gdft_3_v2(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
3083  const Vector3D x[4],
3084  const Matrix3D &invR, /* upper triangular */
3085  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
3086  const double alpha = 1.0/3.0,/* constant */
3087  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
3088  const double delta = 0.0,/* max in denominator */
3089  const double beta = 0.0)/* no modification */
3090  {
3091  double matr[9], f, t1, t2;
3092  double matd[9], g, t3, loc1;
3093  double adjm[6], dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
3094  double J_A[3], J_B[4], J_C[4], J_D[3], J_E[4], J_F[3];
3095  double A[2];
3096 
3097  /* Calculate M = A*inv(R). */
3098  f = x[1][0] - x[0][0];
3099  g = x[2][0] - x[0][0];
3100  t1 = x[3][0] - x[0][0];
3101  matr[0] = f*invR[0][0];
3102  matr[1] = f*invR[0][1] + g*invR[1][1];
3103  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
3104 
3105  f = x[1][1] - x[0][1];
3106  g = x[2][1] - x[0][1];
3107  t1 = x[3][1] - x[0][1];
3108  matr[3] = f*invR[0][0];
3109  matr[4] = f*invR[0][1] + g*invR[1][1];
3110  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
3111 
3112  f = x[1][2] - x[0][2];
3113  g = x[2][2] - x[0][2];
3114  t1 = x[3][2] - x[0][2];
3115  matr[6] = f*invR[0][0];
3116  matr[7] = f*invR[0][1] + g*invR[1][1];
3117  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
3118 
3119  /* Calculate det(M). */
3120  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
3121  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
3122  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
3123  dg[4] = matr[0]*matr[8] - matr[2]*matr[6];
3124  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
3125  dg[7] = matr[2]*matr[3] - matr[0]*matr[5];
3126  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
3127 
3128  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
3129 
3130  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
3131 
3132  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
3133  t2 = t1*t1 + 4.0*delta*delta;
3134  t3 = sqrt(t2);
3135  g = t1 + t3;
3136 
3137  /* Calculate N = M - beta*Q. */
3138  matd[0] = matr[0] - beta*Q[0][0];
3139  matd[1] = matr[1] - beta*Q[0][1];
3140  matd[2] = matr[2] - beta*Q[0][2];
3141  matd[3] = matr[3] - beta*Q[1][0];
3142  matd[4] = matr[4] - beta*Q[1][1];
3143  matd[5] = matr[5] - beta*Q[1][2];
3144  matd[6] = matr[6] - beta*Q[2][0];
3145  matd[7] = matr[7] - beta*Q[2][1];
3146  matd[8] = matr[8] - beta*Q[2][2];
3147 
3148  /* Calculate norm(N) */
3149  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
3150  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
3151  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
3152 
3153  /* Calculate objective function. */
3154  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
3155  obj = f * loc1;
3156 
3157  /* Calculate the derivative of the objective function. */
3158  t3 = 1.0 / t3;
3159  dobj_df = 2.0 * loc1;
3160  dobj_dg = -gamma * obj * t3;
3161  dobj_dfdg = -gamma * dobj_df * t3;
3162  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
3163 
3164  /* Calculate adjoint matrix */
3165  adjm[0] = dobj_df*matd[1] + dobj_dg*dg[1];
3166  adjm[1] = dobj_df*matd[2] + dobj_dg*dg[2];
3167  adjm[2] = dobj_df*matd[4] + dobj_dg*dg[4];
3168  adjm[3] = dobj_df*matd[5] + dobj_dg*dg[5];
3169  adjm[4] = dobj_df*matd[7] + dobj_dg*dg[7];
3170  adjm[5] = dobj_df*matd[8] + dobj_dg*dg[8];
3171 
3172  /* Construct gradients */
3173  g_obj[0] = invR[1][1]*adjm[0]+invR[1][2]*adjm[1];
3174  g_obj[1] = invR[1][1]*adjm[2]+invR[1][2]*adjm[3];
3175  g_obj[2] = invR[1][1]*adjm[4]+invR[1][2]*adjm[5];
3176 
3177  /* Start of the Hessian evaluation */
3178  adjm[0] = dobj_dg*matr[0];
3179  adjm[1] = dobj_dg*matr[3];
3180  adjm[2] = dobj_dg*matr[6];
3181 
3182  matd[1] *= dobj_dfdg;
3183  matd[2] *= dobj_dfdg;
3184  matd[4] *= dobj_dfdg;
3185  matd[5] *= dobj_dfdg;
3186  matd[7] *= dobj_dfdg;
3187  matd[8] *= dobj_dfdg;
3188 
3189  /* Blocks for the Hessian construction */
3190  loc1 = dobj_dgdg*dg[1] + matd[1];
3191  J_A[0] = dobj_df + dg[1]*(matd[1] + loc1);
3192  J_A[1] = dg[1]*matd[2] + loc1*dg[2];
3193  J_B[0] = dg[1]*matd[4] + loc1*dg[4];
3194  J_B[1] = dg[1]*matd[5] + loc1*dg[5] + adjm[2];
3195  J_C[0] = dg[1]*matd[7] + loc1*dg[7];
3196  J_C[1] = dg[1]*matd[8] + loc1*dg[8] - adjm[1];
3197 
3198  loc1 = dobj_dgdg*dg[2] + matd[2];
3199  J_A[2] = dobj_df + dg[2]*(matd[2] + loc1);
3200  J_B[2] = dg[2]*matd[4] + loc1*dg[4] - adjm[2];
3201  J_B[3] = dg[2]*matd[5] + loc1*dg[5];
3202  J_C[2] = dg[2]*matd[7] + loc1*dg[7] + adjm[1];
3203  J_C[3] = dg[2]*matd[8] + loc1*dg[8];
3204 
3205  loc1 = dobj_dgdg*dg[4] + matd[4];
3206  J_D[0] = dobj_df + dg[4]*(matd[4] + loc1);
3207  J_D[1] = dg[4]*matd[5] + loc1*dg[5];
3208  J_E[0] = dg[4]*matd[7] + loc1*dg[7];
3209  J_E[1] = dg[4]*matd[8] + loc1*dg[8] + adjm[0];
3210 
3211  loc1 = dobj_dgdg*dg[5] + matd[5];
3212  J_D[2] = dobj_df + dg[5]*(matd[5] + loc1);
3213  J_E[2] = dg[5]*matd[7] + loc1*dg[7] - adjm[0];
3214  J_E[3] = dg[5]*matd[8] + loc1*dg[8];
3215 
3216  loc1 = dobj_dgdg*dg[7] + matd[7];
3217  J_F[0] = dobj_df + dg[7]*(matd[7] + loc1);
3218  J_F[1] = dg[7]*matd[8] + loc1*dg[8];
3219 
3220  J_F[2] = dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]);
3221 
3222  /* Assemble matrix products */
3223 
3224  /* dx / dx */
3225  A[0] = J_A[0]*invR[1][1] + J_A[1]*invR[1][2];
3226  A[1] = J_A[1]*invR[1][1] + J_A[2]*invR[1][2];
3227  h_obj[0][0] = A[0]*invR[1][1] + A[1]*invR[1][2];
3228 
3229  /* dx / dy */
3230  A[0] = J_B[0]*invR[1][1] + J_B[1]*invR[1][2];
3231  A[1] = J_B[2]*invR[1][1] + J_B[3]*invR[1][2];
3232  h_obj[0][1] = A[0]*invR[1][1] + A[1]*invR[1][2];
3233 
3234  /* dx / dz */
3235  A[0] = J_C[0]*invR[1][1] + J_C[1]*invR[1][2];
3236  A[1] = J_C[2]*invR[1][1] + J_C[3]*invR[1][2];
3237  h_obj[0][2] = A[0]*invR[1][1] + A[1]*invR[1][2];
3238 
3239  /* dy / dy */
3240  A[0] = J_D[0]*invR[1][1] + J_D[1]*invR[1][2];
3241  A[1] = J_D[1]*invR[1][1] + J_D[2]*invR[1][2];
3242  h_obj[1][1] = A[0]*invR[1][1] + A[1]*invR[1][2];
3243 
3244  /* dy / dz */
3245  A[0] = J_E[0]*invR[1][1] + J_E[1]*invR[1][2];
3246  A[1] = J_E[2]*invR[1][1] + J_E[3]*invR[1][2];
3247  h_obj[1][2] = A[0]*invR[1][1] + A[1]*invR[1][2];
3248 
3249  /* dz / dz */
3250  A[0] = J_F[0]*invR[1][1] + J_F[1]*invR[1][2];
3251  A[1] = J_F[1]*invR[1][1] + J_F[2]*invR[1][2];
3252  h_obj[2][2] = A[0]*invR[1][1] + A[1]*invR[1][2];
3253 
3254  /* Complete diagonal blocks */
3255  h_obj.fill_lower_triangle();
3256  return true;
3257  }
3258 
3259  inline bool h_gdft_3_v3(double &obj, Vector3D &g_obj, Matrix3D &h_obj,
3260  const Vector3D x[4],
3261  const Matrix3D &invR, /* upper triangular */
3262  const Matrix3D &Q, /* orthogonal, det(Q) = 1 */
3263  const double alpha = 1.0/3.0,/* constant */
3264  const Exponent& gamma = 2.0/3.0,/* simplicial elements*/
3265  const double delta = 0.0,/* max in denominator */
3266  const double beta = 0.0)/* no modification */
3267  {
3268  double matr[9], f, t1, t2;
3269  double matd[9], g, t3, loc1;
3270  double dg[9], dobj_df, dobj_dfdg, dobj_dg, dobj_dgdg;
3271 
3272  /* Calculate M = A*inv(R). */
3273  f = x[1][0] - x[0][0];
3274  g = x[2][0] - x[0][0];
3275  t1 = x[3][0] - x[0][0];
3276  matr[0] = f*invR[0][0];
3277  matr[1] = f*invR[0][1] + g*invR[1][1];
3278  matr[2] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
3279 
3280  f = x[1][1] - x[0][1];
3281  g = x[2][1] - x[0][1];
3282  t1 = x[3][1] - x[0][1];
3283  matr[3] = f*invR[0][0];
3284  matr[4] = f*invR[0][1] + g*invR[1][1];
3285  matr[5] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
3286 
3287  f = x[1][2] - x[0][2];
3288  g = x[2][2] - x[0][2];
3289  t1 = x[3][2] - x[0][2];
3290  matr[6] = f*invR[0][0];
3291  matr[7] = f*invR[0][1] + g*invR[1][1];
3292  matr[8] = f*invR[0][2] + g*invR[1][2] + t1*invR[2][2];
3293 
3294  /* Calculate det(M). */
3295  dg[0] = matr[4]*matr[8] - matr[5]*matr[7];
3296  dg[1] = matr[5]*matr[6] - matr[3]*matr[8];
3297  dg[2] = matr[3]*matr[7] - matr[4]*matr[6];
3298  dg[5] = matr[1]*matr[6] - matr[0]*matr[7];
3299  dg[8] = matr[0]*matr[4] - matr[1]*matr[3];
3300 
3301  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
3302 
3303  if ((0.0 == delta) && (t1 < MSQ_MIN)) { obj = t1; return false; }
3304 
3305  /* Calculate sqrt(det(M)^2 + 4*delta^2) and denominator. */
3306  t2 = t1*t1 + 4.0*delta*delta;
3307  t3 = sqrt(t2);
3308  g = t1 + t3;
3309 
3310  /* Calculate N = M - beta*Q. */
3311  matd[0] = matr[0] - beta*Q[0][0];
3312  matd[1] = matr[1] - beta*Q[0][1];
3313  matd[2] = matr[2] - beta*Q[0][2];
3314  matd[3] = matr[3] - beta*Q[1][0];
3315  matd[4] = matr[4] - beta*Q[1][1];
3316  matd[5] = matr[5] - beta*Q[1][2];
3317  matd[6] = matr[6] - beta*Q[2][0];
3318  matd[7] = matr[7] - beta*Q[2][1];
3319  matd[8] = matr[8] - beta*Q[2][2];
3320 
3321  /* Calculate norm(N) */
3322  f = matd[0]*matd[0] + matd[1]*matd[1] + matd[2]*matd[2] +
3323  matd[3]*matd[3] + matd[4]*matd[4] + matd[5]*matd[5] +
3324  matd[6]*matd[6] + matd[7]*matd[7] + matd[8]*matd[8];
3325 
3326  /* Calculate objective function. */
3327  loc1 = alpha * pow(2.0, gamma) / pow(g, gamma);
3328  obj = f * loc1;
3329 
3330  /* Calculate the derivative of the objective function. */
3331  t3 = 1.0 / t3;
3332  dobj_df = 2.0 * loc1;
3333  dobj_dg = -gamma * obj * t3;
3334  dobj_dfdg = -gamma * dobj_df * t3;
3335  dobj_dgdg = dobj_dg * ((-gamma - 1.0)*t3 + 4.0*delta*delta/(t2*g));
3336 
3337  /* Construct gradients */
3338  g_obj[0] = invR[2][2]*(dobj_df*matd[2] + dobj_dg*dg[2]);
3339  g_obj[1] = invR[2][2]*(dobj_df*matd[5] + dobj_dg*dg[5]);
3340  g_obj[2] = invR[2][2]*(dobj_df*matd[8] + dobj_dg*dg[8]);
3341 
3342  /* Start of the Hessian evaluation */
3343  t1 = invR[2][2]*invR[2][2];
3344  matd[2] *= dobj_dfdg;
3345  matd[5] *= dobj_dfdg;
3346  matd[8] *= dobj_dfdg;
3347 
3348  /* Blocks for the Hessian construction */
3349  loc1 = dobj_dgdg*dg[2] + matd[2];
3350  h_obj[0][0] = t1*(dobj_df + dg[2]*(matd[2] + loc1));
3351  h_obj[0][1] = t1*(dg[2]*matd[5] + loc1*dg[5]);
3352  h_obj[0][2] = t1*(dg[2]*matd[8] + loc1*dg[8]);
3353 
3354  loc1 = dobj_dgdg*dg[5] + matd[5];
3355  h_obj[1][1] = t1*(dobj_df + dg[5]*(matd[5] + loc1));
3356  h_obj[1][2] = t1*(dg[5]*matd[8] + loc1*dg[8]);
3357 
3358  h_obj[2][2] = t1*(dobj_df + dg[8]*(2.0*matd[8] + dobj_dgdg*dg[8]));
3359 
3360  /* Complete diagonal blocks */
3361  h_obj.fill_lower_triangle();
3362  return true;
3363  }
3364 }
3365 #endif
3366 
3367 
Mesquite::h_gdft_3
bool h_gdft_3(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:815
Mesquite::h_gdft_3_v0
bool h_gdft_3_v0(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:2547
Mesquite::h_gdft_2_v1
bool h_gdft_2_v1(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:1785
Mesquite::g_gdft_2_v1
bool g_gdft_2_v1(double &obj, Vector3D &g_obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:1316
Mesquite::m_gdft_3
bool m_gdft_3(double &obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:643
Mesquite::g_gdft_3_v2
bool g_gdft_3_v2(double &obj, Vector3D &g_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:2374
Mesquite::Vector3D
Vector3D is the object that effeciently stores information about about three-deminsional vectors...
Definition: includeLinks/Vector3D.hpp:64
sqrt
double sqrt(double d)
Definition: double.h:73
Mesquite::h_gdft_2_v0
bool h_gdft_2_v0(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:1497
Mesquite::g_gdft_3_v0
bool g_gdft_3_v0(double &obj, Vector3D &g_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:2173
Mesquite::g_gdft_3_v3
bool g_gdft_3_v3(double &obj, Vector3D &g_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:2466
Mesquite::h_gdft_2
bool h_gdft_2(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6], const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:310
A
rational * A
Definition: vinci_lass.c:67
Mesquite::h_gdft_3_v2
bool h_gdft_3_v2(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:3082
Mesquite::Matrix3D
3*3 Matric class, row-oriented, 0-based [i][j] indexing.
Definition: includeLinks/Matrix3D.hpp:78
Mesquite::h_gdft_3_v1
bool h_gdft_3_v1(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:2865
Mesquite::g_gdft_2_v2
bool g_gdft_2_v2(double &obj, Vector3D &g_obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:1408
Mesquite::m_gdft_2
bool m_gdft_2(double &obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:147
x
void int int REAL * x
Definition: read.cpp:74
n
const NT & n
Definition: rational_rotation.h:72
Mesquite::g_gdft_3_v1
bool g_gdft_3_v1(double &obj, Vector3D &g_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:2279
Mesquite::g_gdft_2_v0
bool g_gdft_2_v0(double &obj, Vector3D &g_obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:1216
Mesquite::h_gdft_2_v2
bool h_gdft_2_v2(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:1999
Mesquite::h_gdft_3_v3
bool h_gdft_3_v3(double &obj, Vector3D &g_obj, Matrix3D &h_obj, const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:3259
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89
Mesquite::g_gdft_2
bool g_gdft_2(double &obj, Vector3D g_obj[3], const Vector3D x[3], const Vector3D &n, const Matrix3D &invR, const Matrix3D &Q, const double alpha=0.5, const Exponent &gamma=1.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:210
Mesquite::MSQ_MIN
const double MSQ_MIN
Definition: Mesquite.hpp:160
Mesquite::g_gdft_3
bool g_gdft_3(double &obj, Vector3D g_obj[4], const Vector3D x[4], const Matrix3D &invR, const Matrix3D &Q, const double alpha=1.0/3.0, const Exponent &gamma=2.0/3.0, const double delta=0.0, const double beta=0.0)
Definition: includeLinks/I_DFTFamilyFunctions.hpp:709