Rocstar  1.0
Rocstar multiphysics simulation application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
QualityMetric/DFT/RI_DFT.cpp
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  ***************************************************************** */
36 #include <cstdio>
37 #include "RI_DFT.hpp"
38 
39 using namespace Mesquite;
40 
41 /*****
42  Functions for:
43  ||T*T' - I||_F^(2b)
44  a * ---------------------------------------
45  (det(T) + sqrt(det(T)^2 + 4*delta^2))^c
46 
47  Note that for this metric, the function is equivalent to:
48  ||T'*T - I||_F^(2b)
49  a * ---------------------------------------
50  (det(T) + sqrt(det(T)^2 + 4*delta^2))^c
51 
52  Note: c can be any value since the denominator is always restricted
53  to be positive. However, b can only take on values of one, two, or
54  any number greater than 2 because the gradient and/or Hessian may
55  not exist in these other cases. In particular, you cannot guarantee
56  that the numerator is bounded away from zero.
57 *****/
58 
59 inline bool m_fcn_ridft2(double &obj,
60  const Vector3D x[3], const Vector3D &n,
61  const Matrix3D &invW,
62  const double a, const Exponent b, const Exponent c,
63  const double delta)
64 {
65  static double matr[9], f, t1, t2;
66  static double fmat[6], g;
67 
68  /* Calculate M = A*inv(W). */
69  f = x[1][0] - x[0][0];
70  g = x[2][0] - x[0][0];
71  t1 = n[0];
72  matr[0] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
73  matr[1] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
74  matr[2] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
75 
76  f = x[1][1] - x[0][1];
77  g = x[2][1] - x[0][1];
78  t1 = n[1];
79  matr[3] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
80  matr[4] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
81  matr[5] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
82 
83  f = x[1][2] - x[0][2];
84  g = x[2][2] - x[0][2];
85  t1 = n[2];
86  matr[6] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
87  matr[7] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
88  matr[8] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
89 
90  /* Calculate det(M). */
91  t1 = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
92  matr[1]*(matr[5]*matr[6] - matr[3]*matr[8]) +
93  matr[2]*(matr[3]*matr[7] - matr[4]*matr[6]);
94  t2 = sqrt(t1*t1 + 4.0*delta*delta);
95  g = t1 + t2;
96  if (g < MSQ_MIN) { obj = g; return false; }
97 
98  /* Calculate norm(M). */
99  fmat[0] = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] - 1.0;
100  fmat[1] = matr[0]*matr[3] + matr[1]*matr[4] + matr[2]*matr[5];
101  fmat[2] = matr[0]*matr[6] + matr[1]*matr[7] + matr[2]*matr[8];
102 
103  fmat[3] = matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] - 1.0;
104  fmat[4] = matr[3]*matr[6] + matr[4]*matr[7] + matr[5]*matr[8];
105 
106  fmat[5] = matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8] - 1.0;
107 
108  f = fmat[0]*fmat[0] + 2.0*fmat[1]*fmat[1] + 2.0*fmat[2]*fmat[2] +
109  fmat[3]*fmat[3] + 2.0*fmat[4]*fmat[4] +
110  fmat[5]*fmat[5];
111 
112  /* Calculate objective function. */
113  obj = a * pow(f, b) * pow(g, c);
114  return true;
115 }
116 
117 inline bool g_fcn_ridft2(double &obj, Vector3D g_obj[3],
118  const Vector3D x[3], const Vector3D &n,
119  const Matrix3D &invW,
120  const double a, const Exponent b, const Exponent c,
121  const double delta)
122 {
123  static double matr[9], f, t1, t2;
124  static double fmat[6], g;
125  static double adj_m[9], df[9], loc1, loc2, loc3;
126 
127  /* Calculate M = A*inv(W). */
128  f = x[1][0] - x[0][0];
129  g = x[2][0] - x[0][0];
130  t1 = n[0];
131  matr[0] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
132  matr[1] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
133  matr[2] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
134 
135  f = x[1][1] - x[0][1];
136  g = x[2][1] - x[0][1];
137  t1 = n[1];
138  matr[3] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
139  matr[4] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
140  matr[5] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
141 
142  f = x[1][2] - x[0][2];
143  g = x[2][2] - x[0][2];
144  t1 = n[2];
145  matr[6] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
146  matr[7] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
147  matr[8] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
148 
149  /* Calculate det(M). */
150  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
151  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
152  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
153  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
154  t2 = sqrt(t1*t1 + 4.0*delta*delta);
155  g = t1 + t2;
156  if (g < MSQ_MIN) { obj = g; return false; }
157 
158  /* Calculate norm(M). */
159  fmat[0] = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] - 1.0;
160  fmat[1] = matr[0]*matr[3] + matr[1]*matr[4] + matr[2]*matr[5];
161  fmat[2] = matr[0]*matr[6] + matr[1]*matr[7] + matr[2]*matr[8];
162 
163  fmat[3] = matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] - 1.0;
164  fmat[4] = matr[3]*matr[6] + matr[4]*matr[7] + matr[5]*matr[8];
165 
166  fmat[5] = matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8] - 1.0;
167 
168  f = fmat[0]*fmat[0] + 2.0*fmat[1]*fmat[1] + 2.0*fmat[2]*fmat[2] +
169  fmat[3]*fmat[3] + 2.0*fmat[4]*fmat[4] +
170  fmat[5]*fmat[5];
171 
172  /* Calculate objective function. */
173  obj = a * pow(f, b) * pow(g, c);
174 
175  /* Calculate the derivative of the objective function. */
176  f = b * obj / f * 4.0; /* Constant on nabla f */
177  g = c * obj / g * (1 + t1 / t2); /* Constant on nabla g */
178 
179  df[0] = fmat[0]*matr[0] + fmat[1]*matr[3] + fmat[2]*matr[6];
180  df[1] = fmat[0]*matr[1] + fmat[1]*matr[4] + fmat[2]*matr[7];
181  df[2] = fmat[0]*matr[2] + fmat[1]*matr[5] + fmat[2]*matr[8];
182 
183  df[3] = fmat[1]*matr[0] + fmat[3]*matr[3] + fmat[4]*matr[6];
184  df[4] = fmat[1]*matr[1] + fmat[3]*matr[4] + fmat[4]*matr[7];
185  df[5] = fmat[1]*matr[2] + fmat[3]*matr[5] + fmat[4]*matr[8];
186 
187  df[6] = fmat[2]*matr[0] + fmat[4]*matr[3] + fmat[5]*matr[6];
188  df[7] = fmat[2]*matr[1] + fmat[4]*matr[4] + fmat[5]*matr[7];
189  df[8] = fmat[2]*matr[2] + fmat[4]*matr[5] + fmat[5]*matr[8];
190 
191  adj_m[0] = df[0]*f + loc1*g;
192  adj_m[1] = df[1]*f + loc2*g;
193  adj_m[2] = df[2]*f + loc3*g;
194 
195  loc1 = matr[0]*g;
196  loc2 = matr[1]*g;
197  loc3 = matr[2]*g;
198 
199  adj_m[3] = df[3]*f + loc3*matr[7] - loc2*matr[8];
200  adj_m[4] = df[4]*f + loc1*matr[8] - loc3*matr[6];
201  adj_m[5] = df[5]*f + loc2*matr[6] - loc1*matr[7];
202 
203  adj_m[6] = df[6]*f + loc2*matr[5] - loc3*matr[4];
204  adj_m[7] = df[7]*f + loc3*matr[3] - loc1*matr[5];
205  adj_m[8] = df[8]*f + loc1*matr[4] - loc2*matr[3];
206 
207  loc1 = invW[0][0]*adj_m[0];
208  loc2 = invW[1][0]*adj_m[0];
209  g_obj[0][0] = -loc1 - loc2;
210  g_obj[1][0] = loc1;
211  g_obj[2][0] = loc2;
212 
213  loc1 = invW[0][1]*adj_m[1];
214  loc2 = invW[1][1]*adj_m[1];
215  g_obj[0][0] -= loc1 + loc2;
216  g_obj[1][0] += loc1;
217  g_obj[2][0] += loc2;
218 
219  loc1 = invW[0][2]*adj_m[2];
220  loc2 = invW[1][2]*adj_m[2];
221  g_obj[0][0] -= loc1 + loc2;
222  g_obj[1][0] += loc1;
223  g_obj[2][0] += loc2;
224 
225  loc1 = invW[0][0]*adj_m[3];
226  loc2 = invW[1][0]*adj_m[3];
227  g_obj[0][1] = -loc1 - loc2;
228  g_obj[1][1] = loc1;
229  g_obj[2][1] = loc2;
230 
231  loc1 = invW[0][1]*adj_m[4];
232  loc2 = invW[1][1]*adj_m[4];
233  g_obj[0][1] -= loc1 + loc2;
234  g_obj[1][1] += loc1;
235  g_obj[2][1] += loc2;
236 
237  loc1 = invW[0][2]*adj_m[5];
238  loc2 = invW[1][2]*adj_m[5];
239  g_obj[0][1] -= loc1 + loc2;
240  g_obj[1][1] += loc1;
241  g_obj[2][1] += loc2;
242 
243  loc1 = invW[0][0]*adj_m[6];
244  loc2 = invW[1][0]*adj_m[6];
245  g_obj[0][2] = -loc1 - loc2;
246  g_obj[1][2] = loc1;
247  g_obj[2][2] = loc2;
248 
249  loc1 = invW[0][1]*adj_m[7];
250  loc2 = invW[1][1]*adj_m[7];
251  g_obj[0][2] -= loc1 + loc2;
252  g_obj[1][2] += loc1;
253  g_obj[2][2] += loc2;
254 
255  loc1 = invW[0][2]*adj_m[8];
256  loc2 = invW[1][2]*adj_m[8];
257  g_obj[0][2] -= loc1 + loc2;
258  g_obj[1][2] += loc1;
259  g_obj[2][2] += loc2;
260  return true;
261 }
262 
263 inline bool h_fcn_ridft2(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6],
264  const Vector3D x[3], const Vector3D &n,
265  const Matrix3D &invW,
266  const double a, const Exponent b, const Exponent c,
267  const double delta)
268 {
269  static double matr[9], f, t1, t2;
270  static double fmat[6], ftmat[6], g, t3, t4;
271  static double adj_m[9], df[9], dg[9], loc0, loc1, loc2, loc3, loc4;
272  static double A[12], J_A[6], J_B[9], J_C[9], cross;
273  static double aux[45];
274 
275  /* Calculate M = A*inv(W). */
276  f = x[1][0] - x[0][0];
277  g = x[2][0] - x[0][0];
278  t1 = n[0];
279  matr[0] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
280  matr[1] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
281  matr[2] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
282 
283  f = x[1][1] - x[0][1];
284  g = x[2][1] - x[0][1];
285  t1 = n[1];
286  matr[3] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
287  matr[4] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
288  matr[5] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
289 
290  f = x[1][2] - x[0][2];
291  g = x[2][2] - x[0][2];
292  t1 = n[2];
293  matr[6] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
294  matr[7] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
295  matr[8] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
296 
297  /* Calculate products for M*M' */
298  aux[ 0] = matr[0]*matr[0];
299  aux[ 1] = matr[0]*matr[1];
300  aux[ 2] = matr[0]*matr[2];
301  aux[ 3] = matr[0]*matr[3];
302  aux[ 4] = matr[0]*matr[4];
303  aux[ 5] = matr[0]*matr[5];
304  aux[ 6] = matr[0]*matr[6];
305  aux[ 7] = matr[0]*matr[7];
306  aux[ 8] = matr[0]*matr[8];
307  aux[ 9] = matr[1]*matr[1];
308  aux[10] = matr[1]*matr[2];
309  aux[11] = matr[1]*matr[3];
310  aux[12] = matr[1]*matr[4];
311  aux[13] = matr[1]*matr[5];
312  aux[14] = matr[1]*matr[6];
313  aux[15] = matr[1]*matr[7];
314  aux[16] = matr[1]*matr[8];
315  aux[17] = matr[2]*matr[2];
316  aux[18] = matr[2]*matr[3];
317  aux[19] = matr[2]*matr[4];
318  aux[20] = matr[2]*matr[5];
319  aux[21] = matr[2]*matr[6];
320  aux[22] = matr[2]*matr[7];
321  aux[23] = matr[2]*matr[8];
322  aux[24] = matr[3]*matr[3];
323  aux[25] = matr[3]*matr[4];
324  aux[26] = matr[3]*matr[5];
325  aux[27] = matr[3]*matr[6];
326  aux[28] = matr[3]*matr[7];
327  aux[29] = matr[3]*matr[8];
328  aux[30] = matr[4]*matr[4];
329  aux[31] = matr[4]*matr[5];
330  aux[32] = matr[4]*matr[6];
331  aux[33] = matr[4]*matr[7];
332  aux[34] = matr[4]*matr[8];
333  aux[35] = matr[5]*matr[5];
334  aux[36] = matr[5]*matr[6];
335  aux[37] = matr[5]*matr[7];
336  aux[38] = matr[5]*matr[8];
337  aux[39] = matr[6]*matr[6];
338  aux[40] = matr[6]*matr[7];
339  aux[41] = matr[6]*matr[8];
340  aux[42] = matr[7]*matr[7];
341  aux[43] = matr[7]*matr[8];
342  aux[44] = matr[8]*matr[8];
343 
344  /* Calculate det(M). */
345  dg[0] = aux[34] - aux[37];
346  dg[1] = aux[36] - aux[29];
347  dg[2] = aux[28] - aux[32];
348  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
349  t2 = t1*t1 + 4.0*delta*delta;
350  t3 = sqrt(t2);
351  g = t1 + t3;
352  if (g < MSQ_MIN) { obj = g; return false; }
353 
354  fmat[0] = aux[ 0] + aux[ 9] + aux[17] - 1.0;
355  fmat[1] = aux[ 3] + aux[12] + aux[20];
356  fmat[2] = aux[ 6] + aux[15] + aux[23];
357 
358  fmat[3] = aux[24] + aux[30] + aux[35] - 1.0;
359  fmat[4] = aux[27] + aux[33] + aux[38];
360 
361  fmat[5] = aux[39] + aux[42] + aux[44] - 1.0;
362 
363  f = fmat[0]*fmat[0] + 2.0*fmat[1]*fmat[1] + 2.0*fmat[2]*fmat[2] +
364  fmat[3]*fmat[3] + 2.0*fmat[4]*fmat[4] +
365  fmat[5]*fmat[5];
366 
367  loc3 = f;
368  loc4 = g;
369 
370  /* Calculate objective function. */
371  obj = a * pow(f, b) * pow(g, c);
372 
373  /* Calculate the derivative of the objective function. */
374  t4 = 1 + t1 / t3;
375 
376  f = b * obj / f * 4.0; /* Constant on nabla f */
377  g = c * obj / g * t4; /* Constant on nabla g */
378 
379  df[0] = fmat[0]*matr[0] + fmat[1]*matr[3] + fmat[2]*matr[6];
380  df[1] = fmat[0]*matr[1] + fmat[1]*matr[4] + fmat[2]*matr[7];
381  df[2] = fmat[0]*matr[2] + fmat[1]*matr[5] + fmat[2]*matr[8];
382 
383  df[3] = fmat[1]*matr[0] + fmat[3]*matr[3] + fmat[4]*matr[6];
384  df[4] = fmat[1]*matr[1] + fmat[3]*matr[4] + fmat[4]*matr[7];
385  df[5] = fmat[1]*matr[2] + fmat[3]*matr[5] + fmat[4]*matr[8];
386 
387  df[6] = fmat[2]*matr[0] + fmat[4]*matr[3] + fmat[5]*matr[6];
388  df[7] = fmat[2]*matr[1] + fmat[4]*matr[4] + fmat[5]*matr[7];
389  df[8] = fmat[2]*matr[2] + fmat[4]*matr[5] + fmat[5]*matr[8];
390 
391  dg[3] = aux[22] - aux[16];
392  dg[4] = aux[ 8] - aux[21];
393  dg[5] = aux[14] - aux[ 7];
394 
395  dg[6] = aux[13] - aux[19];
396  dg[7] = aux[18] - aux[ 5];
397  dg[8] = aux[ 4] - aux[11];
398 
399  adj_m[0] = df[0]*f + dg[0]*g;
400  adj_m[1] = df[1]*f + dg[1]*g;
401  adj_m[2] = df[2]*f + dg[2]*g;
402  adj_m[3] = df[3]*f + dg[3]*g;
403  adj_m[4] = df[4]*f + dg[4]*g;
404  adj_m[5] = df[5]*f + dg[5]*g;
405  adj_m[6] = df[6]*f + dg[6]*g;
406  adj_m[7] = df[7]*f + dg[7]*g;
407  adj_m[8] = df[8]*f + dg[8]*g;
408 
409  loc0 = invW[0][0]*adj_m[0];
410  loc1 = invW[1][0]*adj_m[0];
411  g_obj[0][0] = -loc0 - loc1;
412  g_obj[1][0] = loc0;
413  g_obj[2][0] = loc1;
414 
415  loc0 = invW[0][1]*adj_m[1];
416  loc1 = invW[1][1]*adj_m[1];
417  g_obj[0][0] -= loc0 + loc1;
418  g_obj[1][0] += loc0;
419  g_obj[2][0] += loc1;
420 
421  loc0 = invW[0][2]*adj_m[2];
422  loc1 = invW[1][2]*adj_m[2];
423  g_obj[0][0] -= loc0 + loc1;
424  g_obj[1][0] += loc0;
425  g_obj[2][0] += loc1;
426 
427  loc0 = invW[0][0]*adj_m[3];
428  loc1 = invW[1][0]*adj_m[3];
429  g_obj[0][1] = -loc0 - loc1;
430  g_obj[1][1] = loc0;
431  g_obj[2][1] = loc1;
432 
433  loc0 = invW[0][1]*adj_m[4];
434  loc1 = invW[1][1]*adj_m[4];
435  g_obj[0][1] -= loc0 + loc1;
436  g_obj[1][1] += loc0;
437  g_obj[2][1] += loc1;
438 
439  loc0 = invW[0][2]*adj_m[5];
440  loc1 = invW[1][2]*adj_m[5];
441  g_obj[0][1] -= loc0 + loc1;
442  g_obj[1][1] += loc0;
443  g_obj[2][1] += loc1;
444 
445  loc0 = invW[0][0]*adj_m[6];
446  loc1 = invW[1][0]*adj_m[6];
447  g_obj[0][2] = -loc0 - loc1;
448  g_obj[1][2] = loc0;
449  g_obj[2][2] = loc1;
450 
451  loc0 = invW[0][1]*adj_m[7];
452  loc1 = invW[1][1]*adj_m[7];
453  g_obj[0][2] -= loc0 + loc1;
454  g_obj[1][2] += loc0;
455  g_obj[2][2] += loc1;
456 
457  loc0 = invW[0][2]*adj_m[8];
458  loc1 = invW[1][2]*adj_m[8];
459  g_obj[0][2] -= loc0 + loc1;
460  g_obj[1][2] += loc0;
461  g_obj[2][2] += loc1;
462 
463  /* Start of the Hessian evaluation */
464 
465  ftmat[0] = aux[ 0] + aux[24] + aux[39];
466  ftmat[1] = aux[ 1] + aux[25] + aux[40];
467  ftmat[2] = aux[ 2] + aux[26] + aux[41];
468 
469  ftmat[3] = aux[ 9] + aux[30] + aux[42];
470  ftmat[4] = aux[10] + aux[31] + aux[43];
471 
472  ftmat[5] = aux[17] + aux[35] + aux[44];
473 
474  loc0 = f; /* Constant on nabla^2 f */
475  loc1 = g; /* Constant on nabla^2 g */
476  cross = f * c / loc4 * t4; /* Constant on nabla g nabla f */
477  f = f * (b - 1) / loc3 * 4.0; /* Constant on nabla f nabla f */
478  g = g *((c - 1) * t4 + 4.0*delta*delta / t2) / loc4;
479  /* Constant on nabla g nabla g */
480 
481  /* First block of rows */
482  loc3 = df[0]*f + dg[0]*cross;
483  loc4 = dg[0]*g + df[0]*cross;
484 
485  J_A[0] = loc3*df[0] + loc4*dg[0];
486  J_A[1] = loc3*df[1] + loc4*dg[1];
487  J_A[2] = loc3*df[2] + loc4*dg[2];
488  J_B[0] = loc3*df[3] + loc4*dg[3];
489  J_B[1] = loc3*df[4] + loc4*dg[4];
490  J_B[2] = loc3*df[5] + loc4*dg[5];
491  J_C[0] = loc3*df[6] + loc4*dg[6];
492  J_C[1] = loc3*df[7] + loc4*dg[7];
493  J_C[2] = loc3*df[8] + loc4*dg[8];
494 
495  loc3 = df[1]*f + dg[1]*cross;
496  loc4 = dg[1]*g + df[1]*cross;
497 
498  J_A[3] = loc3*df[1] + loc4*dg[1];
499  J_A[4] = loc3*df[2] + loc4*dg[2];
500  J_B[3] = loc3*df[3] + loc4*dg[3];
501  J_B[4] = loc3*df[4] + loc4*dg[4];
502  J_B[5] = loc3*df[5] + loc4*dg[5];
503  J_C[3] = loc3*df[6] + loc4*dg[6];
504  J_C[4] = loc3*df[7] + loc4*dg[7];
505  J_C[5] = loc3*df[8] + loc4*dg[8];
506 
507  loc3 = df[2]*f + dg[2]*cross;
508  loc4 = dg[2]*g + df[2]*cross;
509 
510  J_A[5] = loc3*df[2] + loc4*dg[2];
511  J_B[6] = loc3*df[3] + loc4*dg[3];
512  J_B[7] = loc3*df[4] + loc4*dg[4];
513  J_B[8] = loc3*df[5] + loc4*dg[5];
514  J_C[6] = loc3*df[6] + loc4*dg[6];
515  J_C[7] = loc3*df[7] + loc4*dg[7];
516  J_C[8] = loc3*df[8] + loc4*dg[8];
517 
518  /* First diagonal block */
519  J_A[0] += loc0*(fmat[0] + ftmat[0] + aux[ 0]);
520  J_A[1] += loc0*( ftmat[1] + aux[ 1]);
521  J_A[2] += loc0*( ftmat[2] + aux[ 2]);
522 
523  J_A[3] += loc0*(fmat[0] + ftmat[3] + aux[ 9]);
524  J_A[4] += loc0*( ftmat[4] + aux[10]);
525 
526  J_A[5] += loc0*(fmat[0] + ftmat[5] + aux[17]);
527 
528  loc2 = invW[0][0]+invW[1][0];
529  loc3 = invW[0][1]+invW[1][1];
530  loc4 = invW[0][2]+invW[1][2];
531 
532  A[0] = -J_A[0]*loc2 - J_A[1]*loc3 - J_A[2]*loc4;
533  A[1] = J_A[0]*invW[0][0] + J_A[1]*invW[0][1] + J_A[2]*invW[0][2];
534  A[2] = J_A[0]*invW[1][0] + J_A[1]*invW[1][1] + J_A[2]*invW[1][2];
535 
536  A[4] = -J_A[1]*loc2 - J_A[3]*loc3 - J_A[4]*loc4;
537  A[5] = J_A[1]*invW[0][0] + J_A[3]*invW[0][1] + J_A[4]*invW[0][2];
538  A[6] = J_A[1]*invW[1][0] + J_A[3]*invW[1][1] + J_A[4]*invW[1][2];
539 
540  A[8] = -J_A[2]*loc2 - J_A[4]*loc3 - J_A[5]*loc4;
541  A[9] = J_A[2]*invW[0][0] + J_A[4]*invW[0][1] + J_A[5]*invW[0][2];
542  A[10] = J_A[2]*invW[1][0] + J_A[4]*invW[1][1] + J_A[5]*invW[1][2];
543 
544  h_obj[0][0][0] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
545  h_obj[1][0][0] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
546  h_obj[2][0][0] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
547 
548  h_obj[3][0][0] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
549  h_obj[4][0][0] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
550 
551  h_obj[5][0][0] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
552 
553  /* First off-diagonal block */
554  J_B[0] += loc0*(fmat[1] + aux[3]);
555  J_B[1] += loc0*aux[11];
556  J_B[2] += loc0*aux[18];
557 
558  J_B[3] += loc0*aux[ 4];
559  J_B[4] += loc0*(fmat[1] + aux[12]);
560  J_B[5] += loc0*aux[19];
561 
562  J_B[6] += loc0*aux[ 5];
563  J_B[7] += loc0*aux[13];
564  J_B[8] += loc0*(fmat[1] + aux[20]);
565 
566  loc2 = matr[8]*loc1;
567  J_B[1] += loc2;
568  J_B[3] -= loc2;
569 
570  loc2 = matr[7]*loc1;
571  J_B[2] -= loc2;
572  J_B[6] += loc2;
573 
574  loc2 = matr[6]*loc1;
575  J_B[5] += loc2;
576  J_B[7] -= loc2;
577 
578  loc2 = invW[0][0]+invW[1][0];
579  loc3 = invW[0][1]+invW[1][1];
580  loc4 = invW[0][2]+invW[1][2];
581 
582  A[0] = -J_B[0]*loc2 - J_B[1]*loc3 - J_B[2]*loc4;
583  A[1] = J_B[0]*invW[0][0] + J_B[1]*invW[0][1] + J_B[2]*invW[0][2];
584  A[2] = J_B[0]*invW[1][0] + J_B[1]*invW[1][1] + J_B[2]*invW[1][2];
585 
586  A[4] = -J_B[3]*loc2 - J_B[4]*loc3 - J_B[5]*loc4;
587  A[5] = J_B[3]*invW[0][0] + J_B[4]*invW[0][1] + J_B[5]*invW[0][2];
588  A[6] = J_B[3]*invW[1][0] + J_B[4]*invW[1][1] + J_B[5]*invW[1][2];
589 
590  A[8] = -J_B[6]*loc2 - J_B[7]*loc3 - J_B[8]*loc4;
591  A[9] = J_B[6]*invW[0][0] + J_B[7]*invW[0][1] + J_B[8]*invW[0][2];
592  A[10] = J_B[6]*invW[1][0] + J_B[7]*invW[1][1] + J_B[8]*invW[1][2];
593 
594  h_obj[0][0][1] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
595  h_obj[1][1][0] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
596  h_obj[2][1][0] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
597 
598  h_obj[1][0][1] = -A[1]*loc2 - A[5]*loc3 - A[9]*loc4;
599  h_obj[3][0][1] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
600  h_obj[4][1][0] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
601 
602  h_obj[2][0][1] = -A[2]*loc2 - A[6]*loc3 - A[10]*loc4;
603  h_obj[4][0][1] = A[2]*invW[0][0] + A[6]*invW[0][1] + A[10]*invW[0][2];
604  h_obj[5][0][1] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
605 
606  /* Second off-diagonal block */
607  J_C[0] += loc0*(fmat[2] + aux[6]);
608  J_C[1] += loc0*aux[14];
609  J_C[2] += loc0*aux[21];
610 
611  J_C[3] += loc0*aux[ 7];
612  J_C[4] += loc0*(fmat[2] + aux[15]);
613  J_C[5] += loc0*aux[22];
614 
615  J_C[6] += loc0*aux[ 8];
616  J_C[7] += loc0*aux[16];
617  J_C[8] += loc0*(fmat[2] + aux[23]);
618 
619  loc2 = matr[5]*loc1;
620  J_C[1] -= loc2;
621  J_C[3] += loc2;
622 
623  loc2 = matr[4]*loc1;
624  J_C[2] += loc2;
625  J_C[6] -= loc2;
626 
627  loc2 = matr[3]*loc1;
628  J_C[5] -= loc2;
629  J_C[7] += loc2;
630 
631  loc2 = invW[0][0]+invW[1][0];
632  loc3 = invW[0][1]+invW[1][1];
633  loc4 = invW[0][2]+invW[1][2];
634 
635  A[0] = -J_C[0]*loc2 - J_C[1]*loc3 - J_C[2]*loc4;
636  A[1] = J_C[0]*invW[0][0] + J_C[1]*invW[0][1] + J_C[2]*invW[0][2];
637  A[2] = J_C[0]*invW[1][0] + J_C[1]*invW[1][1] + J_C[2]*invW[1][2];
638 
639  A[4] = -J_C[3]*loc2 - J_C[4]*loc3 - J_C[5]*loc4;
640  A[5] = J_C[3]*invW[0][0] + J_C[4]*invW[0][1] + J_C[5]*invW[0][2];
641  A[6] = J_C[3]*invW[1][0] + J_C[4]*invW[1][1] + J_C[5]*invW[1][2];
642 
643  A[8] = -J_C[6]*loc2 - J_C[7]*loc3 - J_C[8]*loc4;
644  A[9] = J_C[6]*invW[0][0] + J_C[7]*invW[0][1] + J_C[8]*invW[0][2];
645  A[10] = J_C[6]*invW[1][0] + J_C[7]*invW[1][1] + J_C[8]*invW[1][2];
646 
647  h_obj[0][0][2] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
648  h_obj[1][2][0] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
649  h_obj[2][2][0] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
650 
651  h_obj[1][0][2] = -A[1]*loc2 - A[5]*loc3 - A[9]*loc4;
652  h_obj[3][0][2] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
653  h_obj[4][2][0] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
654 
655  h_obj[2][0][2] = -A[2]*loc2 - A[6]*loc3 - A[10]*loc4;
656  h_obj[4][0][2] = A[2]*invW[0][0] + A[6]*invW[0][1] + A[10]*invW[0][2];
657  h_obj[5][0][2] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
658 
659  /* Second block of rows */
660  loc3 = df[3]*f + dg[3]*cross;
661  loc4 = dg[3]*g + df[3]*cross;
662 
663  J_A[0] = loc3*df[3] + loc4*dg[3];
664  J_A[1] = loc3*df[4] + loc4*dg[4];
665  J_A[2] = loc3*df[5] + loc4*dg[5];
666  J_B[0] = loc3*df[6] + loc4*dg[6];
667  J_B[1] = loc3*df[7] + loc4*dg[7];
668  J_B[2] = loc3*df[8] + loc4*dg[8];
669 
670  loc3 = df[4]*f + dg[4]*cross;
671  loc4 = dg[4]*g + df[4]*cross;
672 
673  J_A[3] = loc3*df[4] + loc4*dg[4];
674  J_A[4] = loc3*df[5] + loc4*dg[5];
675  J_B[3] = loc3*df[6] + loc4*dg[6];
676  J_B[4] = loc3*df[7] + loc4*dg[7];
677  J_B[5] = loc3*df[8] + loc4*dg[8];
678 
679  loc3 = df[5]*f + dg[5]*cross;
680  loc4 = dg[5]*g + df[5]*cross;
681 
682  J_A[5] = loc3*df[5] + loc4*dg[5];
683  J_B[6] = loc3*df[6] + loc4*dg[6];
684  J_B[7] = loc3*df[7] + loc4*dg[7];
685  J_B[8] = loc3*df[8] + loc4*dg[8];
686 
687  /* Second diagonal block */
688  J_A[0] += loc0*(fmat[3] + ftmat[0] + aux[24]);
689  J_A[1] += loc0*( ftmat[1] + aux[25]);
690  J_A[2] += loc0*( ftmat[2] + aux[26]);
691 
692  J_A[3] += loc0*(fmat[3] + ftmat[3] + aux[30]);
693  J_A[4] += loc0*( ftmat[4] + aux[31]);
694 
695  J_A[5] += loc0*(fmat[3] + ftmat[5] + aux[35]);
696 
697  loc2 = invW[0][0]+invW[1][0];
698  loc3 = invW[0][1]+invW[1][1];
699  loc4 = invW[0][2]+invW[1][2];
700 
701  A[0] = -J_A[0]*loc2 - J_A[1]*loc3 - J_A[2]*loc4;
702  A[1] = J_A[0]*invW[0][0] + J_A[1]*invW[0][1] + J_A[2]*invW[0][2];
703  A[2] = J_A[0]*invW[1][0] + J_A[1]*invW[1][1] + J_A[2]*invW[1][2];
704 
705  A[4] = -J_A[1]*loc2 - J_A[3]*loc3 - J_A[4]*loc4;
706  A[5] = J_A[1]*invW[0][0] + J_A[3]*invW[0][1] + J_A[4]*invW[0][2];
707  A[6] = J_A[1]*invW[1][0] + J_A[3]*invW[1][1] + J_A[4]*invW[1][2];
708 
709  A[8] = -J_A[2]*loc2 - J_A[4]*loc3 - J_A[5]*loc4;
710  A[9] = J_A[2]*invW[0][0] + J_A[4]*invW[0][1] + J_A[5]*invW[0][2];
711  A[10] = J_A[2]*invW[1][0] + J_A[4]*invW[1][1] + J_A[5]*invW[1][2];
712 
713  h_obj[0][1][1] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
714  h_obj[1][1][1] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
715  h_obj[2][1][1] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
716 
717  h_obj[3][1][1] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
718  h_obj[4][1][1] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
719 
720  h_obj[5][1][1] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
721 
722  /* Third off-diagonal block */
723  J_B[0] += loc0*(fmat[4] + aux[27]);
724  J_B[1] += loc0*aux[32];
725  J_B[2] += loc0*aux[36];
726 
727  J_B[3] += loc0*aux[28];
728  J_B[4] += loc0*(fmat[4] + aux[33]);
729  J_B[5] += loc0*aux[37];
730 
731  J_B[6] += loc0*aux[29];
732  J_B[7] += loc0*aux[34];
733  J_B[8] += loc0*(fmat[4] + aux[38]);
734 
735  loc2 = matr[2]*loc1;
736  J_B[1] += loc2;
737  J_B[3] -= loc2;
738 
739  loc2 = matr[1]*loc1;
740  J_B[2] -= loc2;
741  J_B[6] += loc2;
742 
743  loc2 = matr[0]*loc1;
744  J_B[5] += loc2;
745  J_B[7] -= loc2;
746 
747  loc2 = invW[0][0]+invW[1][0];
748  loc3 = invW[0][1]+invW[1][1];
749  loc4 = invW[0][2]+invW[1][2];
750 
751  A[0] = -J_B[0]*loc2 - J_B[1]*loc3 - J_B[2]*loc4;
752  A[1] = J_B[0]*invW[0][0] + J_B[1]*invW[0][1] + J_B[2]*invW[0][2];
753  A[2] = J_B[0]*invW[1][0] + J_B[1]*invW[1][1] + J_B[2]*invW[1][2];
754 
755  A[4] = -J_B[3]*loc2 - J_B[4]*loc3 - J_B[5]*loc4;
756  A[5] = J_B[3]*invW[0][0] + J_B[4]*invW[0][1] + J_B[5]*invW[0][2];
757  A[6] = J_B[3]*invW[1][0] + J_B[4]*invW[1][1] + J_B[5]*invW[1][2];
758 
759  A[8] = -J_B[6]*loc2 - J_B[7]*loc3 - J_B[8]*loc4;
760  A[9] = J_B[6]*invW[0][0] + J_B[7]*invW[0][1] + J_B[8]*invW[0][2];
761  A[10] = J_B[6]*invW[1][0] + J_B[7]*invW[1][1] + J_B[8]*invW[1][2];
762 
763  h_obj[0][1][2] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
764  h_obj[1][2][1] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
765  h_obj[2][2][1] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
766 
767  h_obj[1][1][2] = -A[1]*loc2 - A[5]*loc3 - A[9]*loc4;
768  h_obj[3][1][2] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
769  h_obj[4][2][1] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
770 
771  h_obj[2][1][2] = -A[2]*loc2 - A[6]*loc3 - A[10]*loc4;
772  h_obj[4][1][2] = A[2]*invW[0][0] + A[6]*invW[0][1] + A[10]*invW[0][2];
773  h_obj[5][1][2] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
774 
775  /* Third block of rows */
776  loc3 = df[6]*f + dg[6]*cross;
777  loc4 = dg[6]*g + df[6]*cross;
778 
779  J_A[0] = loc3*df[6] + loc4*dg[6];
780  J_A[1] = loc3*df[7] + loc4*dg[7];
781  J_A[2] = loc3*df[8] + loc4*dg[8];
782 
783  loc3 = df[7]*f + dg[7]*cross;
784  loc4 = dg[7]*g + df[7]*cross;
785 
786  J_A[3] = loc3*df[7] + loc4*dg[7];
787  J_A[4] = loc3*df[8] + loc4*dg[8];
788 
789  loc3 = df[8]*f + dg[8]*cross;
790  loc4 = dg[8]*g + df[8]*cross;
791 
792  J_A[5] = loc3*df[8] + loc4*dg[8];
793 
794  /* Third diagonal block */
795  J_A[0] += loc0*(fmat[5] + ftmat[0] + aux[39]);
796  J_A[1] += loc0*( ftmat[1] + aux[40]);
797  J_A[2] += loc0*( ftmat[2] + aux[41]);
798 
799  J_A[3] += loc0*(fmat[5] + ftmat[3] + aux[42]);
800  J_A[4] += loc0*( ftmat[4] + aux[43]);
801 
802  J_A[5] += loc0*(fmat[5] + ftmat[5] + aux[44]);
803 
804  loc2 = invW[0][0]+invW[1][0];
805  loc3 = invW[0][1]+invW[1][1];
806  loc4 = invW[0][2]+invW[1][2];
807 
808  A[0] = -J_A[0]*loc2 - J_A[1]*loc3 - J_A[2]*loc4;
809  A[1] = J_A[0]*invW[0][0] + J_A[1]*invW[0][1] + J_A[2]*invW[0][2];
810  A[2] = J_A[0]*invW[1][0] + J_A[1]*invW[1][1] + J_A[2]*invW[1][2];
811 
812  A[4] = -J_A[1]*loc2 - J_A[3]*loc3 - J_A[4]*loc4;
813  A[5] = J_A[1]*invW[0][0] + J_A[3]*invW[0][1] + J_A[4]*invW[0][2];
814  A[6] = J_A[1]*invW[1][0] + J_A[3]*invW[1][1] + J_A[4]*invW[1][2];
815 
816  A[8] = -J_A[2]*loc2 - J_A[4]*loc3 - J_A[5]*loc4;
817  A[9] = J_A[2]*invW[0][0] + J_A[4]*invW[0][1] + J_A[5]*invW[0][2];
818  A[10] = J_A[2]*invW[1][0] + J_A[4]*invW[1][1] + J_A[5]*invW[1][2];
819 
820  h_obj[0][2][2] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
821  h_obj[1][2][2] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
822  h_obj[2][2][2] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
823 
824  h_obj[3][2][2] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
825  h_obj[4][2][2] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
826 
827  h_obj[5][2][2] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
828 
829  /* Complete diagonal blocks */
830  h_obj[0].fill_lower_triangle();
831  h_obj[3].fill_lower_triangle();
832  h_obj[5].fill_lower_triangle();
833  return true;
834 }
835 
836 inline bool m_fcn_ridft3(double &obj, const Vector3D x[4],
837  const Matrix3D &invW,
838  const double a, const Exponent b, const Exponent c,
839  const double delta)
840 {
841  static double matr[9], f, t1, t2;
842  static double fmat[6], g;
843 
844  /* Calculate M = A*inv(W). */
845  f = x[1][0] - x[0][0];
846  g = x[2][0] - x[0][0];
847  t1 = x[3][0] - x[0][0];
848  matr[0] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
849  matr[1] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
850  matr[2] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
851 
852  f = x[1][1] - x[0][1];
853  g = x[2][1] - x[0][1];
854  t1 = x[3][1] - x[0][1];
855  matr[3] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
856  matr[4] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
857  matr[5] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
858 
859  f = x[1][2] - x[0][2];
860  g = x[2][2] - x[0][2];
861  t1 = x[3][2] - x[0][2];
862  matr[6] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
863  matr[7] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
864  matr[8] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
865 
866  /* Calculate det(M). */
867  t1 = matr[0]*(matr[4]*matr[8] - matr[5]*matr[7]) +
868  matr[1]*(matr[5]*matr[6] - matr[3]*matr[8]) +
869  matr[2]*(matr[3]*matr[7] - matr[4]*matr[6]);
870  t2 = sqrt(t1*t1 + 4.0*delta*delta);
871  g = t1 + t2;
872  if (g < MSQ_MIN) { obj = g; return false; }
873 
874  /* Calculate norm(M). */
875  fmat[0] = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] - 1.0;
876  fmat[1] = matr[0]*matr[3] + matr[1]*matr[4] + matr[2]*matr[5];
877  fmat[2] = matr[0]*matr[6] + matr[1]*matr[7] + matr[2]*matr[8];
878 
879  fmat[3] = matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] - 1.0;
880  fmat[4] = matr[3]*matr[6] + matr[4]*matr[7] + matr[5]*matr[8];
881 
882  fmat[5] = matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8] - 1.0;
883 
884  f = fmat[0]*fmat[0] + 2.0*fmat[1]*fmat[1] + 2.0*fmat[2]*fmat[2] +
885  fmat[3]*fmat[3] + 2.0*fmat[4]*fmat[4] +
886  fmat[5]*fmat[5];
887 
888  /* Calculate objective function. */
889  obj = a * pow(f, b) * pow(g, c);
890  return true;
891 }
892 
893 inline bool g_fcn_ridft3(double &obj, Vector3D g_obj[4], const Vector3D x[4],
894  const Matrix3D &invW,
895  const double a, const Exponent b, const Exponent c,
896  const double delta)
897 {
898  static double matr[9], f, t1, t2;
899  static double fmat[6], g;
900  static double adj_m[9], df[9], loc1, loc2, loc3;
901 
902  /* Calculate M = A*inv(W). */
903  f = x[1][0] - x[0][0];
904  g = x[2][0] - x[0][0];
905  t1 = x[3][0] - x[0][0];
906  matr[0] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
907  matr[1] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
908  matr[2] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
909 
910  f = x[1][1] - x[0][1];
911  g = x[2][1] - x[0][1];
912  t1 = x[3][1] - x[0][1];
913  matr[3] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
914  matr[4] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
915  matr[5] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
916 
917  f = x[1][2] - x[0][2];
918  g = x[2][2] - x[0][2];
919  t1 = x[3][2] - x[0][2];
920  matr[6] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
921  matr[7] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
922  matr[8] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
923 
924  /* Calculate det(M). */
925  loc1 = matr[4]*matr[8] - matr[5]*matr[7];
926  loc2 = matr[5]*matr[6] - matr[3]*matr[8];
927  loc3 = matr[3]*matr[7] - matr[4]*matr[6];
928  t1 = matr[0]*loc1 + matr[1]*loc2 + matr[2]*loc3;
929  t2 = sqrt(t1*t1 + 4.0*delta*delta);
930  g = t1 + t2;
931  if (g < MSQ_MIN) { obj = g; return false; }
932 
933  /* Calculate norm(M). */
934  fmat[0] = matr[0]*matr[0] + matr[1]*matr[1] + matr[2]*matr[2] - 1.0;
935  fmat[1] = matr[0]*matr[3] + matr[1]*matr[4] + matr[2]*matr[5];
936  fmat[2] = matr[0]*matr[6] + matr[1]*matr[7] + matr[2]*matr[8];
937 
938  fmat[3] = matr[3]*matr[3] + matr[4]*matr[4] + matr[5]*matr[5] - 1.0;
939  fmat[4] = matr[3]*matr[6] + matr[4]*matr[7] + matr[5]*matr[8];
940 
941  fmat[5] = matr[6]*matr[6] + matr[7]*matr[7] + matr[8]*matr[8] - 1.0;
942 
943  f = fmat[0]*fmat[0] + 2.0*fmat[1]*fmat[1] + 2.0*fmat[2]*fmat[2] +
944  fmat[3]*fmat[3] + 2.0*fmat[4]*fmat[4] +
945  fmat[5]*fmat[5];
946 
947  /* Compute objective function and constants. Note that this */
948  /* is now done by cases on $b$. We only allow $b=1$, $b=2$, */
949  /* or $b > 2$ in the computations becasue other values do */
950  /* not have defined Hessians when $f = 0$. */
951 
952  if (1 == b) {
953  obj = a * f * pow(g, c);
954  f = a * pow(g, c) * 2.0; /* Constant on nabla f */
955  g = c * obj / g * (1 + t1 / t2); /* Constant on nabla g */
956  }
957  else if (2 == b) {
958  obj = a * f * f * pow(g, c);
959  f = a * f * pow(g, c) * 4.0; /* Constant on nabla f */
960  g = c * obj / g * (1 + t1 / t2); /* Constant on nabla g */
961  }
962  else if (2 < b) {
963  if (0 == f) {
964  obj = 0;
965  f = 0;
966  g = 0;
967  }
968  else {
969  obj = a * pow(f, b) * pow(g, c);
970  f = b * obj / f * 2.0; /* Constant on nabla f */
971  g = c * obj / g * (1 + t1 / t2); /* Constant on nabla g */
972  }
973  }
974  else {
975  printf("b = %5.4e not allowed in RI_DFT\n", (double)b);
976  exit(-1);
977  }
978 
979  df[0] = fmat[0]*matr[0] + fmat[1]*matr[3] + fmat[2]*matr[6];
980  df[1] = fmat[0]*matr[1] + fmat[1]*matr[4] + fmat[2]*matr[7];
981  df[2] = fmat[0]*matr[2] + fmat[1]*matr[5] + fmat[2]*matr[8];
982 
983  df[3] = fmat[1]*matr[0] + fmat[3]*matr[3] + fmat[4]*matr[6];
984  df[4] = fmat[1]*matr[1] + fmat[3]*matr[4] + fmat[4]*matr[7];
985  df[5] = fmat[1]*matr[2] + fmat[3]*matr[5] + fmat[4]*matr[8];
986 
987  df[6] = fmat[2]*matr[0] + fmat[4]*matr[3] + fmat[5]*matr[6];
988  df[7] = fmat[2]*matr[1] + fmat[4]*matr[4] + fmat[5]*matr[7];
989  df[8] = fmat[2]*matr[2] + fmat[4]*matr[5] + fmat[5]*matr[8];
990 
991  adj_m[0] = df[0]*f + loc1*g;
992  adj_m[1] = df[1]*f + loc2*g;
993  adj_m[2] = df[2]*f + loc3*g;
994 
995  loc1 = matr[0]*g;
996  loc2 = matr[1]*g;
997  loc3 = matr[2]*g;
998 
999  adj_m[3] = df[3]*f + loc3*matr[7] - loc2*matr[8];
1000  adj_m[4] = df[4]*f + loc1*matr[8] - loc3*matr[6];
1001  adj_m[5] = df[5]*f + loc2*matr[6] - loc1*matr[7];
1002 
1003  adj_m[6] = df[6]*f + loc2*matr[5] - loc3*matr[4];
1004  adj_m[7] = df[7]*f + loc3*matr[3] - loc1*matr[5];
1005  adj_m[8] = df[8]*f + loc1*matr[4] - loc2*matr[3];
1006 
1007  loc1 = invW[0][0]*adj_m[0];
1008  loc2 = invW[1][0]*adj_m[0];
1009  loc3 = invW[2][0]*adj_m[0];
1010  g_obj[0][0] = -loc1 - loc2 - loc3;
1011  g_obj[1][0] = loc1;
1012  g_obj[2][0] = loc2;
1013  g_obj[3][0] = loc3;
1014 
1015  loc1 = invW[0][1]*adj_m[1];
1016  loc2 = invW[1][1]*adj_m[1];
1017  loc3 = invW[2][1]*adj_m[1];
1018  g_obj[0][0] -= loc1 + loc2 + loc3;
1019  g_obj[1][0] += loc1;
1020  g_obj[2][0] += loc2;
1021  g_obj[3][0] += loc3;
1022 
1023  loc1 = invW[0][2]*adj_m[2];
1024  loc2 = invW[1][2]*adj_m[2];
1025  loc3 = invW[2][2]*adj_m[2];
1026  g_obj[0][0] -= loc1 + loc2 + loc3;
1027  g_obj[1][0] += loc1;
1028  g_obj[2][0] += loc2;
1029  g_obj[3][0] += loc3;
1030 
1031  loc1 = invW[0][0]*adj_m[3];
1032  loc2 = invW[1][0]*adj_m[3];
1033  loc3 = invW[2][0]*adj_m[3];
1034  g_obj[0][1] = -loc1 - loc2 - loc3;
1035  g_obj[1][1] = loc1;
1036  g_obj[2][1] = loc2;
1037  g_obj[3][1] = loc3;
1038 
1039  loc1 = invW[0][1]*adj_m[4];
1040  loc2 = invW[1][1]*adj_m[4];
1041  loc3 = invW[2][1]*adj_m[4];
1042  g_obj[0][1] -= loc1 + loc2 + loc3;
1043  g_obj[1][1] += loc1;
1044  g_obj[2][1] += loc2;
1045  g_obj[3][1] += loc3;
1046 
1047  loc1 = invW[0][2]*adj_m[5];
1048  loc2 = invW[1][2]*adj_m[5];
1049  loc3 = invW[2][2]*adj_m[5];
1050  g_obj[0][1] -= loc1 + loc2 + loc3;
1051  g_obj[1][1] += loc1;
1052  g_obj[2][1] += loc2;
1053  g_obj[3][1] += loc3;
1054 
1055  loc1 = invW[0][0]*adj_m[6];
1056  loc2 = invW[1][0]*adj_m[6];
1057  loc3 = invW[2][0]*adj_m[6];
1058  g_obj[0][2] = -loc1 - loc2 - loc3;
1059  g_obj[1][2] = loc1;
1060  g_obj[2][2] = loc2;
1061  g_obj[3][2] = loc3;
1062 
1063  loc1 = invW[0][1]*adj_m[7];
1064  loc2 = invW[1][1]*adj_m[7];
1065  loc3 = invW[2][1]*adj_m[7];
1066  g_obj[0][2] -= loc1 + loc2 + loc3;
1067  g_obj[1][2] += loc1;
1068  g_obj[2][2] += loc2;
1069  g_obj[3][2] += loc3;
1070 
1071  loc1 = invW[0][2]*adj_m[8];
1072  loc2 = invW[1][2]*adj_m[8];
1073  loc3 = invW[2][2]*adj_m[8];
1074  g_obj[0][2] -= loc1 + loc2 + loc3;
1075  g_obj[1][2] += loc1;
1076  g_obj[2][2] += loc2;
1077  g_obj[3][2] += loc3;
1078  return true;
1079 }
1080 
1081 inline bool h_fcn_ridft3(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10],
1082  const Vector3D x[4], const Matrix3D &invW,
1083  const double a, const Exponent b, const Exponent c,
1084  const double delta)
1085 {
1086  static double matr[9], f, t1, t2;
1087  static double fmat[6], ftmat[6], g, t3, t4;
1088  static double adj_m[9], df[9], dg[9], loc0, loc1, loc2, loc3, loc4;
1089  static double A[12], J_A[6], J_B[9], J_C[9], cross;
1090  static double aux[45];
1091 
1092  /* Calculate M = A*inv(W). */
1093  f = x[1][0] - x[0][0];
1094  g = x[2][0] - x[0][0];
1095  t1 = x[3][0] - x[0][0];
1096  matr[0] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
1097  matr[1] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
1098  matr[2] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
1099 
1100  f = x[1][1] - x[0][1];
1101  g = x[2][1] - x[0][1];
1102  t1 = x[3][1] - x[0][1];
1103  matr[3] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
1104  matr[4] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
1105  matr[5] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
1106 
1107  f = x[1][2] - x[0][2];
1108  g = x[2][2] - x[0][2];
1109  t1 = x[3][2] - x[0][2];
1110  matr[6] = f*invW[0][0] + g*invW[1][0] + t1*invW[2][0];
1111  matr[7] = f*invW[0][1] + g*invW[1][1] + t1*invW[2][1];
1112  matr[8] = f*invW[0][2] + g*invW[1][2] + t1*invW[2][2];
1113 
1114  /* Calculate products for M*M' */
1115  aux[ 0] = matr[0]*matr[0];
1116  aux[ 1] = matr[0]*matr[1];
1117  aux[ 2] = matr[0]*matr[2];
1118  aux[ 3] = matr[0]*matr[3];
1119  aux[ 4] = matr[0]*matr[4];
1120  aux[ 5] = matr[0]*matr[5];
1121  aux[ 6] = matr[0]*matr[6];
1122  aux[ 7] = matr[0]*matr[7];
1123  aux[ 8] = matr[0]*matr[8];
1124  aux[ 9] = matr[1]*matr[1];
1125  aux[10] = matr[1]*matr[2];
1126  aux[11] = matr[1]*matr[3];
1127  aux[12] = matr[1]*matr[4];
1128  aux[13] = matr[1]*matr[5];
1129  aux[14] = matr[1]*matr[6];
1130  aux[15] = matr[1]*matr[7];
1131  aux[16] = matr[1]*matr[8];
1132  aux[17] = matr[2]*matr[2];
1133  aux[18] = matr[2]*matr[3];
1134  aux[19] = matr[2]*matr[4];
1135  aux[20] = matr[2]*matr[5];
1136  aux[21] = matr[2]*matr[6];
1137  aux[22] = matr[2]*matr[7];
1138  aux[23] = matr[2]*matr[8];
1139  aux[24] = matr[3]*matr[3];
1140  aux[25] = matr[3]*matr[4];
1141  aux[26] = matr[3]*matr[5];
1142  aux[27] = matr[3]*matr[6];
1143  aux[28] = matr[3]*matr[7];
1144  aux[29] = matr[3]*matr[8];
1145  aux[30] = matr[4]*matr[4];
1146  aux[31] = matr[4]*matr[5];
1147  aux[32] = matr[4]*matr[6];
1148  aux[33] = matr[4]*matr[7];
1149  aux[34] = matr[4]*matr[8];
1150  aux[35] = matr[5]*matr[5];
1151  aux[36] = matr[5]*matr[6];
1152  aux[37] = matr[5]*matr[7];
1153  aux[38] = matr[5]*matr[8];
1154  aux[39] = matr[6]*matr[6];
1155  aux[40] = matr[6]*matr[7];
1156  aux[41] = matr[6]*matr[8];
1157  aux[42] = matr[7]*matr[7];
1158  aux[43] = matr[7]*matr[8];
1159  aux[44] = matr[8]*matr[8];
1160 
1161  /* Calculate det(M). */
1162  dg[0] = aux[34] - aux[37];
1163  dg[1] = aux[36] - aux[29];
1164  dg[2] = aux[28] - aux[32];
1165  t1 = matr[0]*dg[0] + matr[1]*dg[1] + matr[2]*dg[2];
1166  t2 = t1*t1 + 4.0*delta*delta;
1167  t3 = sqrt(t2);
1168  g = t1 + t3;
1169  if (g < MSQ_MIN) { obj = g; return false; }
1170 
1171  fmat[0] = aux[ 0] + aux[ 9] + aux[17] - 1.0;
1172  fmat[1] = aux[ 3] + aux[12] + aux[20];
1173  fmat[2] = aux[ 6] + aux[15] + aux[23];
1174 
1175  fmat[3] = aux[24] + aux[30] + aux[35] - 1.0;
1176  fmat[4] = aux[27] + aux[33] + aux[38];
1177 
1178  fmat[5] = aux[39] + aux[42] + aux[44] - 1.0;
1179 
1180  f = fmat[0]*fmat[0] + 2.0*fmat[1]*fmat[1] + 2.0*fmat[2]*fmat[2] +
1181  fmat[3]*fmat[3] + 2.0*fmat[4]*fmat[4] +
1182  fmat[5]*fmat[5];
1183 
1184  loc3 = f;
1185  loc4 = g;
1186 
1187  /* Compute objective function and constants. Note that this */
1188  /* is now done by cases on $b$. We only allow $b=1$, $b=2$, */
1189  /* or $b > 2$ in the computations becasue other values do */
1190  /* not have defined Hessians when $f = 0$. */
1191 
1192  t4 = 1 + t1 / t3;
1193  if (1 == b) {
1194  obj = a * f * pow(g, c);
1195  f = a * pow(g, c) * 2.0; /* Constant on nabla f */
1196  g = c * obj / g * t4; /* Constant on nabla g */
1197  }
1198  else if (2 == b) {
1199  obj = a * f * f * pow(g, c);
1200  f = a * f * pow(g, c) * 4.0; /* Constant on nabla f */
1201  g = c * obj / g * t4; /* Constant on nabla g */
1202  }
1203  else if (2 < b) {
1204  if (0 == f) {
1205  obj = 0;
1206  f = 0;
1207  g = 0;
1208  }
1209  else {
1210  obj = a * pow(f, b) * pow(g, c);
1211  f = b * obj / f * 2.0; /* Constant on nabla f */
1212  g = c * obj / g * t4; /* Constant on nabla g */
1213  }
1214  }
1215  else {
1216  printf("b = %5.4e not allowed in RI_DFT\n", (double)b);
1217  exit(-1);
1218  }
1219 
1220  df[0] = fmat[0]*matr[0] + fmat[1]*matr[3] + fmat[2]*matr[6];
1221  df[1] = fmat[0]*matr[1] + fmat[1]*matr[4] + fmat[2]*matr[7];
1222  df[2] = fmat[0]*matr[2] + fmat[1]*matr[5] + fmat[2]*matr[8];
1223 
1224  df[3] = fmat[1]*matr[0] + fmat[3]*matr[3] + fmat[4]*matr[6];
1225  df[4] = fmat[1]*matr[1] + fmat[3]*matr[4] + fmat[4]*matr[7];
1226  df[5] = fmat[1]*matr[2] + fmat[3]*matr[5] + fmat[4]*matr[8];
1227 
1228  df[6] = fmat[2]*matr[0] + fmat[4]*matr[3] + fmat[5]*matr[6];
1229  df[7] = fmat[2]*matr[1] + fmat[4]*matr[4] + fmat[5]*matr[7];
1230  df[8] = fmat[2]*matr[2] + fmat[4]*matr[5] + fmat[5]*matr[8];
1231 
1232  dg[3] = aux[22] - aux[16];
1233  dg[4] = aux[ 8] - aux[21];
1234  dg[5] = aux[14] - aux[ 7];
1235 
1236  dg[6] = aux[13] - aux[19];
1237  dg[7] = aux[18] - aux[ 5];
1238  dg[8] = aux[ 4] - aux[11];
1239 
1240  adj_m[0] = df[0]*f + dg[0]*g;
1241  adj_m[1] = df[1]*f + dg[1]*g;
1242  adj_m[2] = df[2]*f + dg[2]*g;
1243  adj_m[3] = df[3]*f + dg[3]*g;
1244  adj_m[4] = df[4]*f + dg[4]*g;
1245  adj_m[5] = df[5]*f + dg[5]*g;
1246  adj_m[6] = df[6]*f + dg[6]*g;
1247  adj_m[7] = df[7]*f + dg[7]*g;
1248  adj_m[8] = df[8]*f + dg[8]*g;
1249 
1250  loc0 = invW[0][0]*adj_m[0];
1251  loc1 = invW[1][0]*adj_m[0];
1252  loc2 = invW[2][0]*adj_m[0];
1253  g_obj[0][0] = -loc0 - loc1 - loc2;
1254  g_obj[1][0] = loc0;
1255  g_obj[2][0] = loc1;
1256  g_obj[3][0] = loc2;
1257 
1258  loc0 = invW[0][1]*adj_m[1];
1259  loc1 = invW[1][1]*adj_m[1];
1260  loc2 = invW[2][1]*adj_m[1];
1261  g_obj[0][0] -= loc0 + loc1 + loc2;
1262  g_obj[1][0] += loc0;
1263  g_obj[2][0] += loc1;
1264  g_obj[3][0] += loc2;
1265 
1266  loc0 = invW[0][2]*adj_m[2];
1267  loc1 = invW[1][2]*adj_m[2];
1268  loc2 = invW[2][2]*adj_m[2];
1269  g_obj[0][0] -= loc0 + loc1 + loc2;
1270  g_obj[1][0] += loc0;
1271  g_obj[2][0] += loc1;
1272  g_obj[3][0] += loc2;
1273 
1274  loc0 = invW[0][0]*adj_m[3];
1275  loc1 = invW[1][0]*adj_m[3];
1276  loc2 = invW[2][0]*adj_m[3];
1277  g_obj[0][1] = -loc0 - loc1 - loc2;
1278  g_obj[1][1] = loc0;
1279  g_obj[2][1] = loc1;
1280  g_obj[3][1] = loc2;
1281 
1282  loc0 = invW[0][1]*adj_m[4];
1283  loc1 = invW[1][1]*adj_m[4];
1284  loc2 = invW[2][1]*adj_m[4];
1285  g_obj[0][1] -= loc0 + loc1 + loc2;
1286  g_obj[1][1] += loc0;
1287  g_obj[2][1] += loc1;
1288  g_obj[3][1] += loc2;
1289 
1290  loc0 = invW[0][2]*adj_m[5];
1291  loc1 = invW[1][2]*adj_m[5];
1292  loc2 = invW[2][2]*adj_m[5];
1293  g_obj[0][1] -= loc0 + loc1 + loc2;
1294  g_obj[1][1] += loc0;
1295  g_obj[2][1] += loc1;
1296  g_obj[3][1] += loc2;
1297 
1298  loc0 = invW[0][0]*adj_m[6];
1299  loc1 = invW[1][0]*adj_m[6];
1300  loc2 = invW[2][0]*adj_m[6];
1301  g_obj[0][2] = -loc0 - loc1 - loc2;
1302  g_obj[1][2] = loc0;
1303  g_obj[2][2] = loc1;
1304  g_obj[3][2] = loc2;
1305 
1306  loc0 = invW[0][1]*adj_m[7];
1307  loc1 = invW[1][1]*adj_m[7];
1308  loc2 = invW[2][1]*adj_m[7];
1309  g_obj[0][2] -= loc0 + loc1 + loc2;
1310  g_obj[1][2] += loc0;
1311  g_obj[2][2] += loc1;
1312  g_obj[3][2] += loc2;
1313 
1314  loc0 = invW[0][2]*adj_m[8];
1315  loc1 = invW[1][2]*adj_m[8];
1316  loc2 = invW[2][2]*adj_m[8];
1317  g_obj[0][2] -= loc0 + loc1 + loc2;
1318  g_obj[1][2] += loc0;
1319  g_obj[2][2] += loc1;
1320  g_obj[3][2] += loc2;
1321 
1322  /* Start of the Hessian evaluation */
1323 
1324  ftmat[0] = aux[ 0] + aux[24] + aux[39];
1325  ftmat[1] = aux[ 1] + aux[25] + aux[40];
1326  ftmat[2] = aux[ 2] + aux[26] + aux[41];
1327 
1328  ftmat[3] = aux[ 9] + aux[30] + aux[42];
1329  ftmat[4] = aux[10] + aux[31] + aux[43];
1330 
1331  ftmat[5] = aux[17] + aux[35] + aux[44];
1332 
1333  loc0 = f; /* Constant on nabla^2 f */
1334  loc1 = g; /* Constant on nabla^2 g */
1335  if (1 == b) {
1336  cross = f * c / loc4 * t4; /* Constant on nabla g nabla f */
1337  f = 0; /* Constant on nabla f nabla f */
1338  g = g *((c - 1) * t4 + 4.0*delta*delta / t2) / loc4;
1339  /* Constant on nabla g nabla g */
1340  }
1341  else if (2 == b) {
1342  cross = f * c / loc4 * t4; /* Constant on nabla g nabla f */
1343  f = a * pow(loc4, c) * 8.0; /* Constant on nabla f nabla f */
1344  g = g *((c - 1) * t4 + 4.0*delta*delta / t2) / loc4;
1345  /* Constant on nabla g nabla g */
1346  }
1347  else if (2 < b) {
1348  if (0 ==loc3) {
1349  cross = 0;
1350  f = 0;
1351  g = 0;
1352  }
1353  else {
1354  cross = f * c / loc4 * t4; /* Constant on nabla g nabla f */
1355  f = f * (b - 1) / loc3 * 2.0; /* Constant on nabla f nabla f */
1356  g = g *((c - 1) * t4 + 4.0*delta*delta / t2) / loc4;
1357  /* Constant on nabla g nabla g */
1358  }
1359  }
1360  else {
1361  cross = 0;
1362  f = 0;
1363  g = 0;
1364  }
1365 
1366  /* First block of rows */
1367  loc3 = df[0]*f + dg[0]*cross;
1368  loc4 = dg[0]*g + df[0]*cross;
1369 
1370  J_A[0] = loc3*df[0] + loc4*dg[0];
1371  J_A[1] = loc3*df[1] + loc4*dg[1];
1372  J_A[2] = loc3*df[2] + loc4*dg[2];
1373  J_B[0] = loc3*df[3] + loc4*dg[3];
1374  J_B[1] = loc3*df[4] + loc4*dg[4];
1375  J_B[2] = loc3*df[5] + loc4*dg[5];
1376  J_C[0] = loc3*df[6] + loc4*dg[6];
1377  J_C[1] = loc3*df[7] + loc4*dg[7];
1378  J_C[2] = loc3*df[8] + loc4*dg[8];
1379 
1380  loc3 = df[1]*f + dg[1]*cross;
1381  loc4 = dg[1]*g + df[1]*cross;
1382 
1383  J_A[3] = loc3*df[1] + loc4*dg[1];
1384  J_A[4] = loc3*df[2] + loc4*dg[2];
1385  J_B[3] = loc3*df[3] + loc4*dg[3];
1386  J_B[4] = loc3*df[4] + loc4*dg[4];
1387  J_B[5] = loc3*df[5] + loc4*dg[5];
1388  J_C[3] = loc3*df[6] + loc4*dg[6];
1389  J_C[4] = loc3*df[7] + loc4*dg[7];
1390  J_C[5] = loc3*df[8] + loc4*dg[8];
1391 
1392  loc3 = df[2]*f + dg[2]*cross;
1393  loc4 = dg[2]*g + df[2]*cross;
1394 
1395  J_A[5] = loc3*df[2] + loc4*dg[2];
1396  J_B[6] = loc3*df[3] + loc4*dg[3];
1397  J_B[7] = loc3*df[4] + loc4*dg[4];
1398  J_B[8] = loc3*df[5] + loc4*dg[5];
1399  J_C[6] = loc3*df[6] + loc4*dg[6];
1400  J_C[7] = loc3*df[7] + loc4*dg[7];
1401  J_C[8] = loc3*df[8] + loc4*dg[8];
1402 
1403  /* First diagonal block */
1404  J_A[0] += loc0*(fmat[0] + ftmat[0] + aux[ 0]);
1405  J_A[1] += loc0*( ftmat[1] + aux[ 1]);
1406  J_A[2] += loc0*( ftmat[2] + aux[ 2]);
1407 
1408  J_A[3] += loc0*(fmat[0] + ftmat[3] + aux[ 9]);
1409  J_A[4] += loc0*( ftmat[4] + aux[10]);
1410 
1411  J_A[5] += loc0*(fmat[0] + ftmat[5] + aux[17]);
1412 
1413  loc2 = invW[0][0]+invW[1][0]+invW[2][0];
1414  loc3 = invW[0][1]+invW[1][1]+invW[2][1];
1415  loc4 = invW[0][2]+invW[1][2]+invW[2][2];
1416 
1417  A[0] = -J_A[0]*loc2 - J_A[1]*loc3 - J_A[2]*loc4;
1418  A[1] = J_A[0]*invW[0][0] + J_A[1]*invW[0][1] + J_A[2]*invW[0][2];
1419  A[2] = J_A[0]*invW[1][0] + J_A[1]*invW[1][1] + J_A[2]*invW[1][2];
1420  A[3] = J_A[0]*invW[2][0] + J_A[1]*invW[2][1] + J_A[2]*invW[2][2];
1421 
1422  A[4] = -J_A[1]*loc2 - J_A[3]*loc3 - J_A[4]*loc4;
1423  A[5] = J_A[1]*invW[0][0] + J_A[3]*invW[0][1] + J_A[4]*invW[0][2];
1424  A[6] = J_A[1]*invW[1][0] + J_A[3]*invW[1][1] + J_A[4]*invW[1][2];
1425  A[7] = J_A[1]*invW[2][0] + J_A[3]*invW[2][1] + J_A[4]*invW[2][2];
1426 
1427  A[8] = -J_A[2]*loc2 - J_A[4]*loc3 - J_A[5]*loc4;
1428  A[9] = J_A[2]*invW[0][0] + J_A[4]*invW[0][1] + J_A[5]*invW[0][2];
1429  A[10] = J_A[2]*invW[1][0] + J_A[4]*invW[1][1] + J_A[5]*invW[1][2];
1430  A[11] = J_A[2]*invW[2][0] + J_A[4]*invW[2][1] + J_A[5]*invW[2][2];
1431 
1432  h_obj[0][0][0] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
1433  h_obj[1][0][0] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
1434  h_obj[2][0][0] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
1435  h_obj[3][0][0] = A[0]*invW[2][0] + A[4]*invW[2][1] + A[8]*invW[2][2];
1436 
1437  h_obj[4][0][0] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
1438  h_obj[5][0][0] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
1439  h_obj[6][0][0] = A[1]*invW[2][0] + A[5]*invW[2][1] + A[9]*invW[2][2];
1440 
1441  h_obj[7][0][0] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
1442  h_obj[8][0][0] = A[2]*invW[2][0] + A[6]*invW[2][1] + A[10]*invW[2][2];
1443 
1444  h_obj[9][0][0] = A[3]*invW[2][0] + A[7]*invW[2][1] + A[11]*invW[2][2];
1445 
1446  /* First off-diagonal block */
1447  J_B[0] += loc0*(fmat[1] + aux[3]);
1448  J_B[1] += loc0*aux[11];
1449  J_B[2] += loc0*aux[18];
1450 
1451  J_B[3] += loc0*aux[ 4];
1452  J_B[4] += loc0*(fmat[1] + aux[12]);
1453  J_B[5] += loc0*aux[19];
1454 
1455  J_B[6] += loc0*aux[ 5];
1456  J_B[7] += loc0*aux[13];
1457  J_B[8] += loc0*(fmat[1] + aux[20]);
1458 
1459  loc2 = matr[8]*loc1;
1460  J_B[1] += loc2;
1461  J_B[3] -= loc2;
1462 
1463  loc2 = matr[7]*loc1;
1464  J_B[2] -= loc2;
1465  J_B[6] += loc2;
1466 
1467  loc2 = matr[6]*loc1;
1468  J_B[5] += loc2;
1469  J_B[7] -= loc2;
1470 
1471  loc2 = invW[0][0]+invW[1][0]+invW[2][0];
1472  loc3 = invW[0][1]+invW[1][1]+invW[2][1];
1473  loc4 = invW[0][2]+invW[1][2]+invW[2][2];
1474 
1475  A[0] = -J_B[0]*loc2 - J_B[1]*loc3 - J_B[2]*loc4;
1476  A[1] = J_B[0]*invW[0][0] + J_B[1]*invW[0][1] + J_B[2]*invW[0][2];
1477  A[2] = J_B[0]*invW[1][0] + J_B[1]*invW[1][1] + J_B[2]*invW[1][2];
1478  A[3] = J_B[0]*invW[2][0] + J_B[1]*invW[2][1] + J_B[2]*invW[2][2];
1479 
1480  A[4] = -J_B[3]*loc2 - J_B[4]*loc3 - J_B[5]*loc4;
1481  A[5] = J_B[3]*invW[0][0] + J_B[4]*invW[0][1] + J_B[5]*invW[0][2];
1482  A[6] = J_B[3]*invW[1][0] + J_B[4]*invW[1][1] + J_B[5]*invW[1][2];
1483  A[7] = J_B[3]*invW[2][0] + J_B[4]*invW[2][1] + J_B[5]*invW[2][2];
1484 
1485  A[8] = -J_B[6]*loc2 - J_B[7]*loc3 - J_B[8]*loc4;
1486  A[9] = J_B[6]*invW[0][0] + J_B[7]*invW[0][1] + J_B[8]*invW[0][2];
1487  A[10] = J_B[6]*invW[1][0] + J_B[7]*invW[1][1] + J_B[8]*invW[1][2];
1488  A[11] = J_B[6]*invW[2][0] + J_B[7]*invW[2][1] + J_B[8]*invW[2][2];
1489 
1490  h_obj[0][0][1] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
1491  h_obj[1][1][0] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
1492  h_obj[2][1][0] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
1493  h_obj[3][1][0] = A[0]*invW[2][0] + A[4]*invW[2][1] + A[8]*invW[2][2];
1494 
1495  h_obj[1][0][1] = -A[1]*loc2 - A[5]*loc3 - A[9]*loc4;
1496  h_obj[4][0][1] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
1497  h_obj[5][1][0] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
1498  h_obj[6][1][0] = A[1]*invW[2][0] + A[5]*invW[2][1] + A[9]*invW[2][2];
1499 
1500  h_obj[2][0][1] = -A[2]*loc2 - A[6]*loc3 - A[10]*loc4;
1501  h_obj[5][0][1] = A[2]*invW[0][0] + A[6]*invW[0][1] + A[10]*invW[0][2];
1502  h_obj[7][0][1] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
1503  h_obj[8][1][0] = A[2]*invW[2][0] + A[6]*invW[2][1] + A[10]*invW[2][2];
1504 
1505  h_obj[3][0][1] = -A[3]*loc2 - A[7]*loc3 - A[11]*loc4;
1506  h_obj[6][0][1] = A[3]*invW[0][0] + A[7]*invW[0][1] + A[11]*invW[0][2];
1507  h_obj[8][0][1] = A[3]*invW[1][0] + A[7]*invW[1][1] + A[11]*invW[1][2];
1508  h_obj[9][0][1] = A[3]*invW[2][0] + A[7]*invW[2][1] + A[11]*invW[2][2];
1509 
1510  /* Second off-diagonal block */
1511  J_C[0] += loc0*(fmat[2] + aux[6]);
1512  J_C[1] += loc0*aux[14];
1513  J_C[2] += loc0*aux[21];
1514 
1515  J_C[3] += loc0*aux[ 7];
1516  J_C[4] += loc0*(fmat[2] + aux[15]);
1517  J_C[5] += loc0*aux[22];
1518 
1519  J_C[6] += loc0*aux[ 8];
1520  J_C[7] += loc0*aux[16];
1521  J_C[8] += loc0*(fmat[2] + aux[23]);
1522 
1523  loc2 = matr[5]*loc1;
1524  J_C[1] -= loc2;
1525  J_C[3] += loc2;
1526 
1527  loc2 = matr[4]*loc1;
1528  J_C[2] += loc2;
1529  J_C[6] -= loc2;
1530 
1531  loc2 = matr[3]*loc1;
1532  J_C[5] -= loc2;
1533  J_C[7] += loc2;
1534 
1535  loc2 = invW[0][0]+invW[1][0]+invW[2][0];
1536  loc3 = invW[0][1]+invW[1][1]+invW[2][1];
1537  loc4 = invW[0][2]+invW[1][2]+invW[2][2];
1538 
1539  A[0] = -J_C[0]*loc2 - J_C[1]*loc3 - J_C[2]*loc4;
1540  A[1] = J_C[0]*invW[0][0] + J_C[1]*invW[0][1] + J_C[2]*invW[0][2];
1541  A[2] = J_C[0]*invW[1][0] + J_C[1]*invW[1][1] + J_C[2]*invW[1][2];
1542  A[3] = J_C[0]*invW[2][0] + J_C[1]*invW[2][1] + J_C[2]*invW[2][2];
1543 
1544  A[4] = -J_C[3]*loc2 - J_C[4]*loc3 - J_C[5]*loc4;
1545  A[5] = J_C[3]*invW[0][0] + J_C[4]*invW[0][1] + J_C[5]*invW[0][2];
1546  A[6] = J_C[3]*invW[1][0] + J_C[4]*invW[1][1] + J_C[5]*invW[1][2];
1547  A[7] = J_C[3]*invW[2][0] + J_C[4]*invW[2][1] + J_C[5]*invW[2][2];
1548 
1549  A[8] = -J_C[6]*loc2 - J_C[7]*loc3 - J_C[8]*loc4;
1550  A[9] = J_C[6]*invW[0][0] + J_C[7]*invW[0][1] + J_C[8]*invW[0][2];
1551  A[10] = J_C[6]*invW[1][0] + J_C[7]*invW[1][1] + J_C[8]*invW[1][2];
1552  A[11] = J_C[6]*invW[2][0] + J_C[7]*invW[2][1] + J_C[8]*invW[2][2];
1553 
1554  h_obj[0][0][2] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
1555  h_obj[1][2][0] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
1556  h_obj[2][2][0] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
1557  h_obj[3][2][0] = A[0]*invW[2][0] + A[4]*invW[2][1] + A[8]*invW[2][2];
1558 
1559  h_obj[1][0][2] = -A[1]*loc2 - A[5]*loc3 - A[9]*loc4;
1560  h_obj[4][0][2] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
1561  h_obj[5][2][0] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
1562  h_obj[6][2][0] = A[1]*invW[2][0] + A[5]*invW[2][1] + A[9]*invW[2][2];
1563 
1564  h_obj[2][0][2] = -A[2]*loc2 - A[6]*loc3 - A[10]*loc4;
1565  h_obj[5][0][2] = A[2]*invW[0][0] + A[6]*invW[0][1] + A[10]*invW[0][2];
1566  h_obj[7][0][2] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
1567  h_obj[8][2][0] = A[2]*invW[2][0] + A[6]*invW[2][1] + A[10]*invW[2][2];
1568 
1569  h_obj[3][0][2] = -A[3]*loc2 - A[7]*loc3 - A[11]*loc4;
1570  h_obj[6][0][2] = A[3]*invW[0][0] + A[7]*invW[0][1] + A[11]*invW[0][2];
1571  h_obj[8][0][2] = A[3]*invW[1][0] + A[7]*invW[1][1] + A[11]*invW[1][2];
1572  h_obj[9][0][2] = A[3]*invW[2][0] + A[7]*invW[2][1] + A[11]*invW[2][2];
1573 
1574  /* Second block of rows */
1575  loc3 = df[3]*f + dg[3]*cross;
1576  loc4 = dg[3]*g + df[3]*cross;
1577 
1578  J_A[0] = loc3*df[3] + loc4*dg[3];
1579  J_A[1] = loc3*df[4] + loc4*dg[4];
1580  J_A[2] = loc3*df[5] + loc4*dg[5];
1581  J_B[0] = loc3*df[6] + loc4*dg[6];
1582  J_B[1] = loc3*df[7] + loc4*dg[7];
1583  J_B[2] = loc3*df[8] + loc4*dg[8];
1584 
1585  loc3 = df[4]*f + dg[4]*cross;
1586  loc4 = dg[4]*g + df[4]*cross;
1587 
1588  J_A[3] = loc3*df[4] + loc4*dg[4];
1589  J_A[4] = loc3*df[5] + loc4*dg[5];
1590  J_B[3] = loc3*df[6] + loc4*dg[6];
1591  J_B[4] = loc3*df[7] + loc4*dg[7];
1592  J_B[5] = loc3*df[8] + loc4*dg[8];
1593 
1594  loc3 = df[5]*f + dg[5]*cross;
1595  loc4 = dg[5]*g + df[5]*cross;
1596 
1597  J_A[5] = loc3*df[5] + loc4*dg[5];
1598  J_B[6] = loc3*df[6] + loc4*dg[6];
1599  J_B[7] = loc3*df[7] + loc4*dg[7];
1600  J_B[8] = loc3*df[8] + loc4*dg[8];
1601 
1602  /* Second diagonal block */
1603  J_A[0] += loc0*(fmat[3] + ftmat[0] + aux[24]);
1604  J_A[1] += loc0*( ftmat[1] + aux[25]);
1605  J_A[2] += loc0*( ftmat[2] + aux[26]);
1606 
1607  J_A[3] += loc0*(fmat[3] + ftmat[3] + aux[30]);
1608  J_A[4] += loc0*( ftmat[4] + aux[31]);
1609 
1610  J_A[5] += loc0*(fmat[3] + ftmat[5] + aux[35]);
1611 
1612  loc2 = invW[0][0]+invW[1][0]+invW[2][0];
1613  loc3 = invW[0][1]+invW[1][1]+invW[2][1];
1614  loc4 = invW[0][2]+invW[1][2]+invW[2][2];
1615 
1616  A[0] = -J_A[0]*loc2 - J_A[1]*loc3 - J_A[2]*loc4;
1617  A[1] = J_A[0]*invW[0][0] + J_A[1]*invW[0][1] + J_A[2]*invW[0][2];
1618  A[2] = J_A[0]*invW[1][0] + J_A[1]*invW[1][1] + J_A[2]*invW[1][2];
1619  A[3] = J_A[0]*invW[2][0] + J_A[1]*invW[2][1] + J_A[2]*invW[2][2];
1620 
1621  A[4] = -J_A[1]*loc2 - J_A[3]*loc3 - J_A[4]*loc4;
1622  A[5] = J_A[1]*invW[0][0] + J_A[3]*invW[0][1] + J_A[4]*invW[0][2];
1623  A[6] = J_A[1]*invW[1][0] + J_A[3]*invW[1][1] + J_A[4]*invW[1][2];
1624  A[7] = J_A[1]*invW[2][0] + J_A[3]*invW[2][1] + J_A[4]*invW[2][2];
1625 
1626  A[8] = -J_A[2]*loc2 - J_A[4]*loc3 - J_A[5]*loc4;
1627  A[9] = J_A[2]*invW[0][0] + J_A[4]*invW[0][1] + J_A[5]*invW[0][2];
1628  A[10] = J_A[2]*invW[1][0] + J_A[4]*invW[1][1] + J_A[5]*invW[1][2];
1629  A[11] = J_A[2]*invW[2][0] + J_A[4]*invW[2][1] + J_A[5]*invW[2][2];
1630 
1631  h_obj[0][1][1] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
1632  h_obj[1][1][1] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
1633  h_obj[2][1][1] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
1634  h_obj[3][1][1] = A[0]*invW[2][0] + A[4]*invW[2][1] + A[8]*invW[2][2];
1635 
1636  h_obj[4][1][1] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
1637  h_obj[5][1][1] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
1638  h_obj[6][1][1] = A[1]*invW[2][0] + A[5]*invW[2][1] + A[9]*invW[2][2];
1639 
1640  h_obj[7][1][1] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
1641  h_obj[8][1][1] = A[2]*invW[2][0] + A[6]*invW[2][1] + A[10]*invW[2][2];
1642 
1643  h_obj[9][1][1] = A[3]*invW[2][0] + A[7]*invW[2][1] + A[11]*invW[2][2];
1644 
1645  /* Third off-diagonal block */
1646  J_B[0] += loc0*(fmat[4] + aux[27]);
1647  J_B[1] += loc0*aux[32];
1648  J_B[2] += loc0*aux[36];
1649 
1650  J_B[3] += loc0*aux[28];
1651  J_B[4] += loc0*(fmat[4] + aux[33]);
1652  J_B[5] += loc0*aux[37];
1653 
1654  J_B[6] += loc0*aux[29];
1655  J_B[7] += loc0*aux[34];
1656  J_B[8] += loc0*(fmat[4] + aux[38]);
1657 
1658  loc2 = matr[2]*loc1;
1659  J_B[1] += loc2;
1660  J_B[3] -= loc2;
1661 
1662  loc2 = matr[1]*loc1;
1663  J_B[2] -= loc2;
1664  J_B[6] += loc2;
1665 
1666  loc2 = matr[0]*loc1;
1667  J_B[5] += loc2;
1668  J_B[7] -= loc2;
1669 
1670  loc2 = invW[0][0]+invW[1][0]+invW[2][0];
1671  loc3 = invW[0][1]+invW[1][1]+invW[2][1];
1672  loc4 = invW[0][2]+invW[1][2]+invW[2][2];
1673 
1674  A[0] = -J_B[0]*loc2 - J_B[1]*loc3 - J_B[2]*loc4;
1675  A[1] = J_B[0]*invW[0][0] + J_B[1]*invW[0][1] + J_B[2]*invW[0][2];
1676  A[2] = J_B[0]*invW[1][0] + J_B[1]*invW[1][1] + J_B[2]*invW[1][2];
1677  A[3] = J_B[0]*invW[2][0] + J_B[1]*invW[2][1] + J_B[2]*invW[2][2];
1678 
1679  A[4] = -J_B[3]*loc2 - J_B[4]*loc3 - J_B[5]*loc4;
1680  A[5] = J_B[3]*invW[0][0] + J_B[4]*invW[0][1] + J_B[5]*invW[0][2];
1681  A[6] = J_B[3]*invW[1][0] + J_B[4]*invW[1][1] + J_B[5]*invW[1][2];
1682  A[7] = J_B[3]*invW[2][0] + J_B[4]*invW[2][1] + J_B[5]*invW[2][2];
1683 
1684  A[8] = -J_B[6]*loc2 - J_B[7]*loc3 - J_B[8]*loc4;
1685  A[9] = J_B[6]*invW[0][0] + J_B[7]*invW[0][1] + J_B[8]*invW[0][2];
1686  A[10] = J_B[6]*invW[1][0] + J_B[7]*invW[1][1] + J_B[8]*invW[1][2];
1687  A[11] = J_B[6]*invW[2][0] + J_B[7]*invW[2][1] + J_B[8]*invW[2][2];
1688 
1689  h_obj[0][1][2] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
1690  h_obj[1][2][1] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
1691  h_obj[2][2][1] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
1692  h_obj[3][2][1] = A[0]*invW[2][0] + A[4]*invW[2][1] + A[8]*invW[2][2];
1693 
1694  h_obj[1][1][2] = -A[1]*loc2 - A[5]*loc3 - A[9]*loc4;
1695  h_obj[4][1][2] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
1696  h_obj[5][2][1] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
1697  h_obj[6][2][1] = A[1]*invW[2][0] + A[5]*invW[2][1] + A[9]*invW[2][2];
1698 
1699  h_obj[2][1][2] = -A[2]*loc2 - A[6]*loc3 - A[10]*loc4;
1700  h_obj[5][1][2] = A[2]*invW[0][0] + A[6]*invW[0][1] + A[10]*invW[0][2];
1701  h_obj[7][1][2] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
1702  h_obj[8][2][1] = A[2]*invW[2][0] + A[6]*invW[2][1] + A[10]*invW[2][2];
1703 
1704  h_obj[3][1][2] = -A[3]*loc2 - A[7]*loc3 - A[11]*loc4;
1705  h_obj[6][1][2] = A[3]*invW[0][0] + A[7]*invW[0][1] + A[11]*invW[0][2];
1706  h_obj[8][1][2] = A[3]*invW[1][0] + A[7]*invW[1][1] + A[11]*invW[1][2];
1707  h_obj[9][1][2] = A[3]*invW[2][0] + A[7]*invW[2][1] + A[11]*invW[2][2];
1708 
1709  /* Third block of rows */
1710  loc3 = df[6]*f + dg[6]*cross;
1711  loc4 = dg[6]*g + df[6]*cross;
1712 
1713  J_A[0] = loc3*df[6] + loc4*dg[6];
1714  J_A[1] = loc3*df[7] + loc4*dg[7];
1715  J_A[2] = loc3*df[8] + loc4*dg[8];
1716 
1717  loc3 = df[7]*f + dg[7]*cross;
1718  loc4 = dg[7]*g + df[7]*cross;
1719 
1720  J_A[3] = loc3*df[7] + loc4*dg[7];
1721  J_A[4] = loc3*df[8] + loc4*dg[8];
1722 
1723  loc3 = df[8]*f + dg[8]*cross;
1724  loc4 = dg[8]*g + df[8]*cross;
1725 
1726  J_A[5] = loc3*df[8] + loc4*dg[8];
1727 
1728  /* Third diagonal block */
1729  J_A[0] += loc0*(fmat[5] + ftmat[0] + aux[39]);
1730  J_A[1] += loc0*( ftmat[1] + aux[40]);
1731  J_A[2] += loc0*( ftmat[2] + aux[41]);
1732 
1733  J_A[3] += loc0*(fmat[5] + ftmat[3] + aux[42]);
1734  J_A[4] += loc0*( ftmat[4] + aux[43]);
1735 
1736  J_A[5] += loc0*(fmat[5] + ftmat[5] + aux[44]);
1737 
1738  loc2 = invW[0][0]+invW[1][0]+invW[2][0];
1739  loc3 = invW[0][1]+invW[1][1]+invW[2][1];
1740  loc4 = invW[0][2]+invW[1][2]+invW[2][2];
1741 
1742  A[0] = -J_A[0]*loc2 - J_A[1]*loc3 - J_A[2]*loc4;
1743  A[1] = J_A[0]*invW[0][0] + J_A[1]*invW[0][1] + J_A[2]*invW[0][2];
1744  A[2] = J_A[0]*invW[1][0] + J_A[1]*invW[1][1] + J_A[2]*invW[1][2];
1745  A[3] = J_A[0]*invW[2][0] + J_A[1]*invW[2][1] + J_A[2]*invW[2][2];
1746 
1747  A[4] = -J_A[1]*loc2 - J_A[3]*loc3 - J_A[4]*loc4;
1748  A[5] = J_A[1]*invW[0][0] + J_A[3]*invW[0][1] + J_A[4]*invW[0][2];
1749  A[6] = J_A[1]*invW[1][0] + J_A[3]*invW[1][1] + J_A[4]*invW[1][2];
1750  A[7] = J_A[1]*invW[2][0] + J_A[3]*invW[2][1] + J_A[4]*invW[2][2];
1751 
1752  A[8] = -J_A[2]*loc2 - J_A[4]*loc3 - J_A[5]*loc4;
1753  A[9] = J_A[2]*invW[0][0] + J_A[4]*invW[0][1] + J_A[5]*invW[0][2];
1754  A[10] = J_A[2]*invW[1][0] + J_A[4]*invW[1][1] + J_A[5]*invW[1][2];
1755  A[11] = J_A[2]*invW[2][0] + J_A[4]*invW[2][1] + J_A[5]*invW[2][2];
1756 
1757  h_obj[0][2][2] = -A[0]*loc2 - A[4]*loc3 - A[8]*loc4;
1758  h_obj[1][2][2] = A[0]*invW[0][0] + A[4]*invW[0][1] + A[8]*invW[0][2];
1759  h_obj[2][2][2] = A[0]*invW[1][0] + A[4]*invW[1][1] + A[8]*invW[1][2];
1760  h_obj[3][2][2] = A[0]*invW[2][0] + A[4]*invW[2][1] + A[8]*invW[2][2];
1761 
1762  h_obj[4][2][2] = A[1]*invW[0][0] + A[5]*invW[0][1] + A[9]*invW[0][2];
1763  h_obj[5][2][2] = A[1]*invW[1][0] + A[5]*invW[1][1] + A[9]*invW[1][2];
1764  h_obj[6][2][2] = A[1]*invW[2][0] + A[5]*invW[2][1] + A[9]*invW[2][2];
1765 
1766  h_obj[7][2][2] = A[2]*invW[1][0] + A[6]*invW[1][1] + A[10]*invW[1][2];
1767  h_obj[8][2][2] = A[2]*invW[2][0] + A[6]*invW[2][1] + A[10]*invW[2][2];
1768 
1769  h_obj[9][2][2] = A[3]*invW[2][0] + A[7]*invW[2][1] + A[11]*invW[2][2];
1770 
1771  /* Complete diagonal blocks */
1772  h_obj[0].fill_lower_triangle();
1773  h_obj[4].fill_lower_triangle();
1774  h_obj[7].fill_lower_triangle();
1775  h_obj[9].fill_lower_triangle();
1776  return true;
1777 }
1778 
1779 #if 0
1780 bool RI_DFT::evaluate_element(PatchData& pd,
1781  MsqMeshEntity* element,
1782  double& value, MsqError &err)
1783 {
1784  Matrix3D T[MSQ_MAX_NUM_VERT_PER_ENT];
1785  double c_k[MSQ_MAX_NUM_VERT_PER_ENT];
1786  double dft[MSQ_MAX_NUM_VERT_PER_ENT];
1787  bool return_flag;
1788  double h, tau;
1789 
1790  size_t num_T = element->vertex_count();
1791  compute_T_matrices(*element, pd, T, num_T, c_k, err); MSQ_ERRZERO(err);
1792 
1793  const double id[] = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
1794  const Matrix3D I(id);
1795  Matrix3D TT;
1796  for (size_t i=0; i<num_T; ++i) {
1797  tau = det(T[i]);
1798  TT = transpose(T[i]);
1799  TT = TT * T[i];
1800  TT -= I;
1801  dft[i] = .5 * Frobenius_2(TT);
1802  return_flag = get_barrier_function(pd, tau, h, err); MSQ_ERRZERO(err);
1803  dft[i] /= pow(h, 4.0/3.0);
1804  }
1805 
1806  value = weighted_average_metrics(c_k, dft, num_T, err); MSQ_ERRZERO(err);
1807 
1808  return return_flag;
1809 }
1810 #endif
1811 
1812 bool RI_DFT::evaluate_element(PatchData& pd,
1813  MsqMeshEntity* e,
1814  double& m,
1815  MsqError &err)
1816 {
1817  // Only works with the weighted average
1818  MsqError mErr;
1819  MsqVertex *vertices = pd.get_vertex_array(err); MSQ_ERRZERO(err);
1820 
1821  EntityTopology topo = e->get_element_type();
1822 
1823  const size_t nv = e->vertex_count();
1824  const size_t *v_i = e->get_vertex_index_array();
1825 
1826  size_t idx = pd.get_element_index(e);
1827  const TargetMatrix *W = pd.targetMatrices.get_element_corner_tags(&pd, idx, err );
1828  MSQ_ERRZERO(err);
1829 
1830  // Initialize constants for the metric
1831  const double delta = pd.get_barrier_delta(err); MSQ_ERRZERO(err);
1832 
1833  const int tetInd[4][4] = {{0, 1, 2, 3}, {1, 2, 0, 3},
1834  {2, 0, 1, 3}, {3, 2, 1, 0}};
1835  const int hexInd[8][4] = {{0, 1, 3, 4}, {1, 2, 0, 5},
1836  {2, 3, 1, 6}, {3, 0, 2, 7},
1837  {4, 7, 5, 0}, {5, 4, 6, 1},
1838  {6, 5, 7, 2}, {7, 6, 4, 3}};
1839 
1840  // Variables used for computing the metric
1841  double mMetric; // Metric value
1842  bool mValid; // Validity of the metric
1843  int i, j;
1844 
1845  m = 0.0;
1846  switch(topo) {
1847  case TRIANGLE:
1848 
1849  for (i = 0; i < 3; ++i) {
1850  for (j = 0; j < 3; ++j) {
1851  mCoords[j] = vertices[v_i[tetInd[i][j]]];
1852  }
1853 
1854  pd.get_domain_normal_at_vertex(v_i[tetInd[i][0]], true, mNormal, mErr);
1855 
1856  if (mErr || mNormal.length() == 0) {
1857  mNormal = (vertices[v_i[tetInd[i][1]]] - vertices[v_i[tetInd[i][0]]]) *
1858  (vertices[v_i[tetInd[i][2]]] - vertices[v_i[tetInd[i][0]]]);
1859  mNormal.normalize();
1860  }
1861  mNormal *= MSQ_3RT_2_OVER_6RT_3;
1862 
1863  inv(invW, W[i]);
1864 
1865  mValid = m_fcn_ridft2(mMetric, mCoords, mNormal, invW, a, b, c, delta);
1866  if (!mValid) return false;
1867  m += W[i].get_cK() * mMetric;
1868  }
1869 
1870  m *= MSQ_ONE_THIRD;
1871  break;
1872 
1873  case QUADRILATERAL:
1874 
1875  for (i = 0; i < 4; ++i) {
1876  for (j = 0; j < 3; ++j) {
1877  mCoords[j] = vertices[v_i[hexInd[i][j]]];
1878  }
1879 
1880  pd.get_domain_normal_at_vertex(v_i[hexInd[i][0]], true, mNormal, mErr);
1881  if (mErr || mNormal.length() == 0) {
1882  mNormal = (vertices[v_i[hexInd[i][1]]] - vertices[v_i[hexInd[i][0]]]) *
1883  (vertices[v_i[hexInd[i][2]]] - vertices[v_i[hexInd[i][0]]]);
1884  mNormal.normalize();
1885  }
1886 
1887  inv(invW, W[i]);
1888 
1889  mValid = m_fcn_ridft2(mMetric, mCoords, mNormal, invW, a, b, c, delta);
1890  if (!mValid) return false;
1891  m += W[i].get_cK() * mMetric;
1892  }
1893 
1894  m *= 0.25;
1895  break;
1896 
1897  case TETRAHEDRON:
1898 
1899  for (i = 0; i < 4; ++i) {
1900  for (j = 0; j < 4; ++j) {
1901  mCoords[j] = vertices[v_i[tetInd[i][j]]];
1902  }
1903 
1904  inv(invW, W[i]);
1905 
1906  mValid = m_fcn_ridft3(mMetric, mCoords, invW, a, b, c, delta);
1907  if (!mValid) return false;
1908  m += W[i].get_cK() * mMetric;
1909  }
1910 
1911  m *= 0.25;
1912  break;
1913 
1914  case HEXAHEDRON:
1915 
1916  for (i = 0; i < 8; ++i) {
1917  for (j = 0; j < 4; ++j) {
1918  mCoords[j] = vertices[v_i[hexInd[i][j]]];
1919  }
1920 
1921  inv(invW, W[i]);
1922 
1923  mValid = m_fcn_ridft3(mMetric, mCoords, invW, a, b, c, delta);
1924  if (!mValid) return false;
1925  m += W[i].get_cK() * mMetric;
1926  }
1927 
1928  m *= 0.125;
1929  break;
1930 
1931  default:
1932  MSQ_SETERR(err)("element type not implemented.", MsqError::NOT_IMPLEMENTED);
1933  return false;
1934  }
1935 
1936  return true;
1937 }
1938 
1939 bool RI_DFT::compute_element_analytical_gradient(PatchData &pd,
1940  MsqMeshEntity *e,
1941  MsqVertex *fv[],
1942  Vector3D g[],
1943  int nfv,
1944  double &m,
1945  MsqError &err)
1946 {
1947  // Only works with the weighted average
1948 
1949  MsqVertex *vertices = pd.get_vertex_array(err); MSQ_ERRZERO(err);
1950 
1951  EntityTopology topo = e->get_element_type();
1952 
1953  const size_t nv = e->vertex_count();
1954  const size_t *v_i = e->get_vertex_index_array();
1955 
1956  size_t idx = pd.get_element_index(e);
1957  const TargetMatrix *W = pd.targetMatrices.get_element_corner_tags(&pd, idx, err );
1958  MSQ_ERRZERO(err);
1959 
1960  // Initialize constants for the metric
1961  const double delta = pd.get_barrier_delta(err); MSQ_ERRZERO(err);
1962 
1963  const int tetInd[4][4] = {{0, 1, 2, 3}, {1, 2, 0, 3},
1964  {2, 0, 1, 3}, {3, 2, 1, 0}};
1965  const int hexInd[8][4] = {{0, 1, 3, 4}, {1, 2, 0, 5},
1966  {2, 3, 1, 6}, {3, 0, 2, 7},
1967  {4, 7, 5, 0}, {5, 4, 6, 1},
1968  {6, 5, 7, 2}, {7, 6, 4, 3}};
1969 
1970  // Variables used for computing the metric
1971  double mMetric; // Metric value
1972  bool mValid; // Validity of the metric
1973  int i, j;
1974 
1975  m = 0.0;
1976  switch(topo) {
1977  case TRIANGLE:
1978  assert(3 == nv);
1979 
1980 #ifndef ANALYTIC
1981  mValid = compute_element_numerical_gradient(pd, e, fv, g, nfv, m, err);
1982  return !MSQ_CHKERR(err) && mValid;
1983 #else
1984 
1985  // The following analytic calculation only works correctly if the
1986  // normal is constant. If the normal is not constaint, you need
1987  // to get the gradient of the normal with respect to the vertex
1988  // positions to obtain the correct values.
1989 
1990  for (i = 0; i < 3; ++i) {
1991  mAccGrads[i] = 0.0;
1992  }
1993 
1994  for (i = 0; i < 3; ++i) {
1995  for (j = 0; j < 3; ++j) {
1996  mCoords[j] = vertices[v_i[tetInd[i][j]]];
1997  }
1998 
1999  pd.get_domain_normal_at_vertex(v_i[tetInd[i][0]], true, mNormal, err);
2000  MSQ_ERRZERO(err);
2001  mNormal *= MSQ_3RT_2_OVER_6RT_3);
2002 
2003  inv(invW, W[i]);
2004 
2005  mValid = g_fcn_ridft2(mMetric, mGrads, mCoords, mNormal,
2006  invW, a, b, c, delta);
2007  if (!mValid) return false;
2008  m += W[i].get_cK() * mMetric;
2009 
2010  for (j = 0; j < 3; ++j) {
2011  mAccGrads[tetInd[i][j]] += W[i].get_cK() * mGrads[j];
2012  }
2013  }
2014 
2015  m *= MSQ_ONE_THIRD;
2016  for (i = 0; i < 3; ++i) {
2017  mAccGrads[i] *= MSQ_ONE_THIRD;
2018  }
2019 
2020  // This is not very efficient, but is one way to select correct gradients.
2021  // For gradients, info is returned only for free vertices, in the order
2022  // of fv[].
2023 
2024  for (i = 0; i < 3; ++i) {
2025  for (j = 0; j < nfv; ++j) {
2026  if (vertices + v_i[i] == fv[j]) {
2027  g[j] = mAccGrads[i];
2028  }
2029  }
2030  }
2031 #endif
2032 
2033  break;
2034 
2035  case QUADRILATERAL:
2036 
2037 #ifndef ANALYTIC
2038  mValid = compute_element_numerical_gradient(pd, e, fv, g, nfv, m, err);
2039  return !MSQ_CHKERR(err) && mValid;
2040 #else
2041 
2042  // The following analytic calculation only works correctly if the
2043  // normal is constant. If the normal is not constaint, you need
2044  // to get the gradient of the normal with respect to the vertex
2045  // positions to obtain the correct values.
2046 
2047  for (i = 0; i < 4; ++i) {
2048  mAccGrads[i] = 0.0;
2049  }
2050 
2051  for (i = 0; i < 4; ++i) {
2052  for (j = 0; j < 3; ++j) {
2053  mCoords[j] = vertices[v_i[hexInd[i][j]]];
2054  }
2055 
2056  pd.get_domain_normal_at_vertex(v_i[hexInd[i][0]], true, mNormal, err);
2057  MSQ_ERRZERO(err);
2058 
2059  inv(invW, W[i]);
2060 
2061  mValid = g_fcn_ridft2(mMetric, mGrads, mCoords, mNormal,
2062  invW, a, b, c, delta);
2063  if (!mValid) return false;
2064  m += W[i].get_cK() * mMetric;
2065 
2066  for (j = 0; j < 3; ++j) {
2067  mAccGrads[hexInd[i][j]] += W[i].get_cK() * mGrads[j];
2068  }
2069  }
2070 
2071  m *= 0.25;
2072  for (i = 0; i < 4; ++i) {
2073  mAccGrads[i] *= 0.25;
2074  }
2075 
2076  // This is not very efficient, but is one way to select correct gradients
2077  // For gradients, info is returned only for free vertices, in the order
2078  // of fv[].
2079 
2080  for (i = 0; i < 4; ++i) {
2081  for (j = 0; j < nfv; ++j) {
2082  if (vertices + v_i[i] == fv[j]) {
2083  g[j] = mAccGrads[i];
2084  }
2085  }
2086  }
2087 #endif
2088 
2089  break;
2090 
2091  case TETRAHEDRON:
2092 
2093  for (i = 0; i < 4; ++i) {
2094  mAccGrads[i] = 0.0;
2095  }
2096 
2097  for (i = 0; i < 4; ++i) {
2098  for (j = 0; j < 4; ++j) {
2099  mCoords[j] = vertices[v_i[tetInd[i][j]]];
2100  }
2101 
2102  inv(invW, W[i]);
2103 
2104  mValid = g_fcn_ridft3(mMetric, mGrads, mCoords, invW, a, b, c, delta);
2105  if (!mValid) return false;
2106  m += W[i].get_cK() * mMetric;
2107 
2108  for (j = 0; j < 4; ++j) {
2109  mAccGrads[tetInd[i][j]] += W[i].get_cK() * mGrads[j];
2110  }
2111  }
2112 
2113  m *= 0.25;
2114  for (i = 0; i < 4; ++i) {
2115  mAccGrads[i] *= 0.25;
2116  }
2117 
2118  // This is not very efficient, but is one way to select correct gradients.
2119  // For gradients, info is returned only for free vertices, in the order
2120  // of fv[].
2121 
2122  for (i = 0; i < 4; ++i) {
2123  for (size_t k = 0; k < nv; ++k) {
2124  if (vertices + v_i[i] == fv[k]) {
2125  g[k] = mAccGrads[i];
2126  }
2127  }
2128  }
2129  break;
2130 
2131  case HEXAHEDRON:
2132 
2133  for (i = 0; i < 8; ++i) {
2134  mAccGrads[i] = 0.0;
2135  }
2136 
2137  for (i = 0; i < 8; ++i) {
2138  for (j = 0; j < 4; ++j) {
2139  mCoords[j] = vertices[v_i[hexInd[i][j]]];
2140  }
2141 
2142  inv(invW, W[i]);
2143 
2144  mValid = g_fcn_ridft3(mMetric, mGrads, mCoords, invW, a, b, c, delta);
2145  if (!mValid) return false;
2146  m += W[i].get_cK() * mMetric;
2147 
2148  for (j = 0; j < 4; ++j) {
2149  mAccGrads[hexInd[i][j]] += W[i].get_cK() * mGrads[j];
2150  }
2151  }
2152 
2153  m *= 0.125;
2154  for (i = 0; i < 8; ++i) {
2155  mAccGrads[i] *= 0.125;
2156  }
2157 
2158  // This is not very efficient, but is one way to select correct gradients
2159  // For gradients, info is returned only for free vertices, in the order
2160  // of fv[].
2161 
2162  for (i = 0; i < 8; ++i) {
2163  for (j = 0; j < nfv; ++j) {
2164  if (vertices + v_i[i] == fv[j]) {
2165  g[j] = mAccGrads[i];
2166  }
2167  }
2168  }
2169  break;
2170 
2171  default:
2172  MSQ_SETERR(err)("element type not implemented.",MsqError::NOT_IMPLEMENTED);
2173  return false;
2174  }
2175 
2176  return true;
2177 }
2178 
2179 bool RI_DFT::compute_element_analytical_hessian(PatchData &pd,
2180  MsqMeshEntity *e,
2181  MsqVertex *fv[],
2182  Vector3D g[],
2183  Matrix3D h[],
2184  int nfv,
2185  double &m,
2186  MsqError &err)
2187 {
2188  // Only works with the weighted average
2189 
2190  MsqVertex *vertices = pd.get_vertex_array(err); MSQ_ERRZERO(err);
2191 
2192  EntityTopology topo = e->get_element_type();
2193 
2194  const size_t nv = e->vertex_count();
2195  const size_t *v_i = e->get_vertex_index_array();
2196 
2197  size_t idx = pd.get_element_index(e);
2198  const TargetMatrix *W = pd.targetMatrices.get_element_corner_tags(&pd, idx, err );
2199  MSQ_ERRZERO(err);
2200 
2201  // Initialize constants for the metric
2202  const double delta = pd.get_barrier_delta(err); MSQ_ERRZERO(err);
2203 
2204  const int tetInd[4][4] = {{0, 1, 2, 3}, {1, 2, 0, 3},
2205  {2, 0, 1, 3}, {3, 2, 1, 0}};
2206  const int hexInd[8][4] = {{0, 1, 3, 4}, {1, 2, 0, 5},
2207  {2, 3, 1, 6}, {3, 0, 2, 7},
2208  {4, 7, 5, 0}, {5, 4, 6, 1},
2209  {6, 5, 7, 2}, {7, 6, 4, 3}};
2210 
2211  // Variables used for computing the metric
2212  double mMetric; // Metric value
2213  bool mValid; // Validity of the metric
2214  int i, j, k, l;
2215  int row, col, loc;
2216 
2217  m = 0.0;
2218  switch(topo) {
2219  case TRIANGLE:
2220  assert(3 == nv);
2221 
2222 #ifndef ANALYTIC
2223  mValid = compute_element_numerical_hessian(pd, e, fv, g, h, nfv, m, err);
2224  return !MSQ_CHKERR(err) && mValid;
2225 #else
2226 
2227  // The following analytic calculation only works correctly if the
2228  // normal is constant. If the normal is not constaint, you need
2229  // to get the gradient of the normal with respect to the vertex
2230  // positions to obtain the correct values.
2231 
2232  // Zero out the hessian and gradient vector
2233  for (i = 0; i < 3; ++i) {
2234  g[i] = 0.0;
2235  }
2236 
2237  for (i = 0; i < 6; ++i) {
2238  h[i].zero();
2239  }
2240 
2241  // Compute the metric and sum them together
2242  for (i = 0; i < 3; ++i) {
2243  for (j = 0; j < 3; ++j) {
2244  mCoords[j] = vertices[v_i[tetInd[i][j]]];
2245  }
2246 
2247  pd.get_domain_normal_at_vertex(v_i[tetInd[i][0]], true, mNormal, err);
2248  MSQ_ERRZERO(err);
2249  mNormal *= MSQ_3RT_2_OVER_6RT_3;
2250 
2251  inv(invW, W[i]);
2252 
2253  mValid = h_fcn_ridft2(mMetric, mGrads, mHessians, mCoords, mNormal,
2254  invW, a, b, c, delta);
2255  if (!mValid) return false;
2256 
2257  m += W[i].get_cK() * mMetric;
2258 
2259  for (j = 0; j < 3; ++j) {
2260  g[tetInd[i][j]] += W[i].get_cK() * mGrads[j];
2261  }
2262 
2263  l = 0;
2264  for (j = 0; j < 3; ++j) {
2265  for (k = j; k < 3; ++k) {
2266  row = tetInd[i][j];
2267  col = tetInd[i][k];
2268 
2269  if (row <= col) {
2270  loc = 3*row - (row*(row+1)/2) + col;
2271  h[loc] += W[i].get_cK() * mHessians[l];
2272  }
2273  else {
2274  loc = 3*col - (col*(col+1)/2) + row;
2275  h[loc] += W[i].get_cK() * transpose(mHessians[l]);
2276  }
2277  ++l;
2278  }
2279  }
2280  }
2281 
2282  m *= MSQ_ONE_THIRD;
2283  for (i = 0; i < 3; ++i) {
2284  g[i] *= MSQ_ONE_THIRD;
2285  }
2286 
2287  for (i = 0; i < 6; ++i) {
2288  h[i] *= MSQ_ONE_THIRD;
2289  }
2290 
2291  // zero out fixed elements of g
2292  j = 0;
2293  for (i = 0; i < 3; ++i) {
2294  if (vertices + v_i[i] == fv[j]) {
2295  // if free vertex, see next
2296  ++j;
2297  }
2298  else {
2299  // else zero gradient entry and hessian entries.
2300  g[i] = 0.;
2301 
2302  switch(i) {
2303  case 0:
2304  h[0].zero(); h[1].zero(); h[2].zero();
2305  break;
2306 
2307  case 1:
2308  h[1].zero(); h[3].zero(); h[4].zero();
2309  break;
2310 
2311  case 2:
2312  h[2].zero(); h[4].zero(); h[5].zero();
2313  break;
2314  }
2315  }
2316  }
2317 #endif
2318 
2319  break;
2320 
2321  case QUADRILATERAL:
2322 
2323 #ifndef ANALYTIC
2324  mValid = compute_element_numerical_hessian(pd, e, fv, g, h, nfv, m, err);
2325  return !MSQ_CHKERR(err) && mValid;
2326 #else
2327 
2328  // The following analytic calculation only works correctly if the
2329  // normal is constant. If the normal is not constaint, you need
2330  // to get the gradient of the normal with respect to the vertex
2331  // positions to obtain the correct values.
2332 
2333  // Zero out the hessian and gradient vector
2334  for (i = 0; i < 4; ++i) {
2335  g[i] = 0.0;
2336  }
2337 
2338  for (i = 0; i < 10; ++i) {
2339  h[i].zero();
2340  }
2341 
2342  // Compute the metric and sum them together
2343  for (i = 0; i < 4; ++i) {
2344  for (j = 0; j < 3; ++j) {
2345  mCoords[j] = vertices[v_i[hexInd[i][j]]];
2346  }
2347 
2348  pd.get_domain_normal_at_vertex(v_i[hexInd[i][0]], true, mNormal, err);
2349  MSQ_ERRZERO(err);
2350 
2351  inv(invW, W[i]);
2352 
2353  mValid = h_fcn_ridft2(mMetric, mGrads, mHessians, mCoords, mNormal,
2354  invW, a, b, c, delta);
2355  if (!mValid) return false;
2356 
2357  m += W[i].get_cK() * mMetric;
2358 
2359  for (j = 0; j < 3; ++j) {
2360  g[hexInd[i][j]] += W[i].get_cK() * mGrads[j];
2361  }
2362 
2363  l = 0;
2364  for (j = 0; j < 3; ++j) {
2365  for (k = j; k < 3; ++k) {
2366  row = hexInd[i][j];
2367  col = hexInd[i][k];
2368 
2369  if (row <= col) {
2370  loc = 4*row - (row*(row+1)/2) + col;
2371  h[loc] += W[i].get_cK() * mHessians[l];
2372  }
2373  else {
2374  loc = 4*col - (col*(col+1)/2) + row;
2375  h[loc] += W[i].get_cK() * transpose(mHessians[l]);
2376  }
2377  ++l;
2378  }
2379  }
2380  }
2381 
2382  m *= 0.25;
2383  for (i = 0; i < 4; ++i) {
2384  g[i] *= 0.25;
2385  }
2386 
2387  for (i = 0; i < 10; ++i) {
2388  h[i] *= 0.25;
2389  }
2390 
2391  // zero out fixed elements of gradient and Hessian
2392  j = 0;
2393  for (i = 0; i < 4; ++i) {
2394  if (vertices + v_i[i] == fv[j]) {
2395  // if free vertex, see next
2396  ++j;
2397  }
2398  else {
2399  // else zero gradient entry and hessian entries.
2400  g[i] = 0.;
2401 
2402  switch(i) {
2403  case 0:
2404  h[0].zero(); h[1].zero(); h[2].zero(); h[3].zero();
2405  break;
2406 
2407  case 1:
2408  h[1].zero(); h[4].zero(); h[5].zero(); h[6].zero();
2409  break;
2410 
2411  case 2:
2412  h[2].zero(); h[5].zero(); h[7].zero(); h[8].zero();
2413  break;
2414 
2415  case 3:
2416  h[3].zero(); h[6].zero(); h[8].zero(); h[9].zero();
2417  break;
2418  }
2419  }
2420  }
2421 #endif
2422 
2423  break;
2424 
2425  case TETRAHEDRON:
2426 
2427  // Zero out the hessian and gradient vector
2428  for (i = 0; i < 4; ++i) {
2429  g[i] = 0.0;
2430  }
2431 
2432  for (i = 0; i < 10; ++i) {
2433  h[i].zero();
2434  }
2435 
2436  // Compute the metric and sum them together
2437  for (i = 0; i < 4; ++i) {
2438  for (j = 0; j < 4; ++j) {
2439  mCoords[j] = vertices[v_i[tetInd[i][j]]];
2440  }
2441 
2442  inv(invW, W[i]);
2443 
2444  mValid = h_fcn_ridft3(mMetric, mGrads, mHessians, mCoords,
2445  invW, a, b, c, delta);
2446  if (!mValid) return false;
2447 
2448  m += W[i].get_cK() * mMetric;
2449 
2450  for (j = 0; j < 4; ++j) {
2451  g[tetInd[i][j]] += W[i].get_cK() * mGrads[j];
2452  }
2453 
2454  l = 0;
2455  for (j = 0; j < 4; ++j) {
2456  for (k = j; k < 4; ++k) {
2457  row = tetInd[i][j];
2458  col = tetInd[i][k];
2459 
2460  if (row <= col) {
2461  loc = 4*row - (row*(row+1)/2) + col;
2462  h[loc] += W[i].get_cK() * mHessians[l];
2463  }
2464  else {
2465  loc = 4*col - (col*(col+1)/2) + row;
2466  h[loc] += W[i].get_cK() * transpose(mHessians[l]);
2467  }
2468  ++l;
2469  }
2470  }
2471  }
2472 
2473  m *= 0.25;
2474  for (i = 0; i < 4; ++i) {
2475  g[i] *= 0.25;
2476  }
2477 
2478  for (i = 0; i < 10; ++i) {
2479  h[i] *= 0.25;
2480  }
2481 
2482  // zero out fixed elements of g
2483  j = 0;
2484  for (i = 0; i < 4; ++i) {
2485  if (vertices + v_i[i] == fv[j]) {
2486  // if free vertex, see next
2487  ++j;
2488  }
2489  else {
2490  // else zero gradient entry and hessian entries.
2491  g[i] = 0.;
2492 
2493  switch(i) {
2494  case 0:
2495  h[0].zero(); h[1].zero(); h[2].zero(); h[3].zero();
2496  break;
2497 
2498  case 1:
2499  h[1].zero(); h[4].zero(); h[5].zero(); h[6].zero();
2500  break;
2501 
2502  case 2:
2503  h[2].zero(); h[5].zero(); h[7].zero(); h[8].zero();
2504  break;
2505 
2506  case 3:
2507  h[3].zero(); h[6].zero(); h[8].zero(); h[9].zero();
2508  break;
2509  }
2510  }
2511  }
2512  break;
2513 
2514  case HEXAHEDRON:
2515 
2516  // Zero out the hessian and gradient vector
2517  for (i = 0; i < 8; ++i) {
2518  g[i] = 0.0;
2519  }
2520 
2521  for (i = 0; i < 36; ++i) {
2522  h[i].zero();
2523  }
2524 
2525  // Compute the metric and sum them together
2526  for (i = 0; i < 8; ++i) {
2527  for (j = 0; j < 4; ++j) {
2528  mCoords[j] = vertices[v_i[hexInd[i][j]]];
2529  }
2530 
2531  inv(invW, W[i]);
2532 
2533  mValid = h_fcn_ridft3(mMetric, mGrads, mHessians, mCoords,
2534  invW, a, b, c, delta);
2535  if (!mValid) return false;
2536 
2537  m += W[i].get_cK() * mMetric;
2538 
2539  for (j = 0; j < 4; ++j) {
2540  g[hexInd[i][j]] += W[i].get_cK() * mGrads[j];
2541  }
2542 
2543  l = 0;
2544  for (j = 0; j < 4; ++j) {
2545  for (k = j; k < 4; ++k) {
2546  row = hexInd[i][j];
2547  col = hexInd[i][k];
2548 
2549  if (row <= col) {
2550  loc = 8*row - (row*(row+1)/2) + col;
2551  h[loc] += W[i].get_cK() * mHessians[l];
2552  }
2553  else {
2554  loc = 8*col - (col*(col+1)/2) + row;
2555  h[loc] += W[i].get_cK() * transpose(mHessians[l]);
2556  }
2557  ++l;
2558  }
2559  }
2560  }
2561 
2562  m *= 0.125;
2563  for (i = 0; i < 8; ++i) {
2564  g[i] *= 0.125;
2565  }
2566 
2567  for (i = 0; i < 36; ++i) {
2568  h[i] *= 0.125;
2569  }
2570 
2571  // zero out fixed elements of gradient and Hessian
2572  j = 0;
2573  for (i = 0; i < 8; ++i) {
2574  if (vertices + v_i[i] == fv[j]) {
2575  // if free vertex, see next
2576  ++j;
2577  }
2578  else {
2579  // else zero gradient entry and hessian entries.
2580  g[i] = 0.;
2581 
2582  switch(i) {
2583  case 0:
2584  h[0].zero(); h[1].zero(); h[2].zero(); h[3].zero();
2585  h[4].zero(); h[5].zero(); h[6].zero(); h[7].zero();
2586  break;
2587 
2588  case 1:
2589  h[1].zero(); h[8].zero(); h[9].zero(); h[10].zero();
2590  h[11].zero(); h[12].zero(); h[13].zero(); h[14].zero();
2591  break;
2592 
2593  case 2:
2594  h[2].zero(); h[9].zero(); h[15].zero(); h[16].zero();
2595  h[17].zero(); h[18].zero(); h[19].zero(); h[20].zero();
2596  break;
2597 
2598  case 3:
2599  h[3].zero(); h[10].zero(); h[16].zero(); h[21].zero();
2600  h[22].zero(); h[23].zero(); h[24].zero(); h[25].zero();
2601  break;
2602 
2603  case 4:
2604  h[4].zero(); h[11].zero(); h[17].zero(); h[22].zero();
2605  h[26].zero(); h[27].zero(); h[28].zero(); h[29].zero();
2606  break;
2607 
2608  case 5:
2609  h[5].zero(); h[12].zero(); h[18].zero(); h[23].zero();
2610  h[27].zero(); h[30].zero(); h[31].zero(); h[32].zero();
2611  break;
2612 
2613  case 6:
2614  h[6].zero(); h[13].zero(); h[19].zero(); h[24].zero();
2615  h[28].zero(); h[31].zero(); h[33].zero(); h[34].zero();
2616  break;
2617 
2618  case 7:
2619  h[7].zero(); h[14].zero(); h[20].zero(); h[25].zero();
2620  h[29].zero(); h[32].zero(); h[34].zero(); h[35].zero();
2621  break;
2622  }
2623  }
2624  }
2625  break;
2626 
2627  default:
2628  MSQ_SETERR(err)("element type not implemented.",MsqError::NOT_IMPLEMENTED);
2629  return false;
2630  }
2631 
2632  return true;
2633 }
MSQ_ERRZERO
#define MSQ_ERRZERO(err)
Return zero/NULL on error.
Definition: includeLinks/MsqError.hpp:72
RI_DFT.hpp
Mesquite::MSQ_ONE_THIRD
static const double MSQ_ONE_THIRD
Definition: Mesquite.hpp:128
Mesquite::RI_DFT::c
Exponent c
Definition: includeLinks/RI_DFT.hpp:96
Mesquite::QualityMetric::compute_element_numerical_gradient
bool compute_element_numerical_gradient(PatchData &pd, MsqMeshEntity *element, MsqVertex *free_vtces[], Vector3D grad_vec[], int num_free_vtx, double &metric_value, MsqError &err)
Non-virtual function which numerically computes the gradient of a QualityMetric of a given element fo...
Definition: QualityMetric/QualityMetric.cpp:262
Mesquite::Matrix3D::zero
void zero()
Sets all entries to zero (more efficient than assignement).
Definition: includeLinks/Matrix3D.hpp:167
nvc::cross
NVec< 3, T > cross(const NVec< 3, T > &u, const NVec< 3, T > &v)
Definition: Rocon/include/NVec.h:185
Mesquite::QualityMetric::compute_element_numerical_hessian
bool compute_element_numerical_hessian(PatchData &pd, MsqMeshEntity *element, MsqVertex *free_vtces[], Vector3D grad_vec[], Matrix3D hessian[], int num_free_vtx, double &metric_value, MsqError &err)
Definition: QualityMetric/QualityMetric.cpp:337
Mesquite::PatchData::targetMatrices
CornerTag< TargetMatrix > targetMatrices
Target matrix data.
Definition: includeLinks/PatchData.hpp:427
k
j indices k indices k
Definition: Indexing.h:6
Mesquite::MsqError
Used to hold the error state and return it to the application.
Definition: includeLinks/MsqError.hpp:106
Mesquite::EntityTopology
EntityTopology
Definition: Mesquite.hpp:92
Mesquite::RI_DFT::a
double a
Definition: includeLinks/RI_DFT.hpp:94
Mesquite::transpose
Matrix3D transpose(const Matrix3D &A)
Definition: includeLinks/Matrix3D.hpp:335
Mesquite::MsqError::NOT_IMPLEMENTED
requested functionality is not (yet) implemented
Definition: includeLinks/MsqError.hpp:128
Mesquite::RI_DFT::mNormal
Vector3D mNormal
Definition: includeLinks/RI_DFT.hpp:99
Mesquite::MsqMeshEntity
MsqMeshEntity is the Mesquite object that stores information about the elements in the mesh...
Definition: includeLinks/MsqMeshEntity.hpp:69
Mesquite::Vector3D
Vector3D is the object that effeciently stores information about about three-deminsional vectors...
Definition: includeLinks/Vector3D.hpp:64
Mesquite::MSQ_MAX_NUM_VERT_PER_ENT
const int MSQ_MAX_NUM_VERT_PER_ENT
Definition: Mesquite.hpp:120
sqrt
double sqrt(double d)
Definition: double.h:73
Mesquite::RI_DFT::compute_element_analytical_gradient
bool compute_element_analytical_gradient(PatchData &pd, MsqMeshEntity *e, MsqVertex *fv[], Vector3D g[], int nfv, double &m, MsqError &err)
Virtual function that computes the gradient of the QualityMetric analytically. The base class impleme...
Definition: QualityMetric/DFT/RI_DFT.cpp:1939
Mesquite::Frobenius_2
double Frobenius_2(const Matrix3D &A)
Return the square of the Frobenius norm of A, i.e. sum (diag (A&#39; * A))
Definition: includeLinks/Matrix3D.hpp:326
Mesquite::RI_DFT::compute_element_analytical_hessian
bool compute_element_analytical_hessian(PatchData &pd, MsqMeshEntity *e, MsqVertex *fv[], Vector3D g[], Matrix3D h[], int nfv, double &m, MsqError &err)
Definition: QualityMetric/DFT/RI_DFT.cpp:2179
A
rational * A
Definition: vinci_lass.c:67
Mesquite::MsqMeshEntity::vertex_count
msq_stdc::size_t vertex_count() const
Returns the number of vertices in this element, based on its element type.
Definition: includeLinks/MsqMeshEntity.hpp:185
Mesquite::PatchData::get_barrier_delta
double get_barrier_delta(MsqError &err)
Returns delta based on the minimum and maximum corner determinant over all elements in the patch This...
Definition: Mesh/PatchData.cpp:117
MSQ_CHKERR
#define MSQ_CHKERR(err)
Mesquite&#39;s Error Checking macro.
Definition: includeLinks/MsqError.hpp:62
Mesquite::PatchData::get_element_index
size_t get_element_index(MsqMeshEntity *element)
Definition: includeLinks/PatchData.hpp:601
Mesquite::Matrix3D
3*3 Matric class, row-oriented, 0-based [i][j] indexing.
Definition: includeLinks/Matrix3D.hpp:78
Mesquite::QualityMetric::weighted_average_metrics
double weighted_average_metrics(const double coef[], const double metric_values[], const int &num_values, MsqError &err)
takes an array of coefficients and an array of metrics (both of length num_value) and averages the co...
Definition: includeLinks/QualityMetric.hpp:708
MSQ_SETERR
#define MSQ_SETERR(err)
Macro to set error - use err.clear() to clear.
Definition: includeLinks/MsqError.hpp:83
Mesquite::RI_DFT::b
Exponent b
Definition: includeLinks/RI_DFT.hpp:95
i
blockLoc i
Definition: read.cpp:79
x
void int int REAL * x
Definition: read.cpp:74
Mesquite::DistanceFromTarget::compute_T_matrices
void compute_T_matrices(MsqMeshEntity &elem, PatchData &pd, Matrix3D T[], size_t num_T, double c_k[], MsqError &err)
For a given element, compute each corner matrix A, and given a target corner matrix W...
Definition: includeLinks/DistanceFromTarget.hpp:74
n
const NT & n
Definition: rational_rotation.h:72
m_fcn_ridft3
bool m_fcn_ridft3(double &obj, const Vector3D x[4], const Matrix3D &invW, const double a, const Exponent b, const Exponent c, const double delta)
Definition: QualityMetric/DFT/RI_DFT.cpp:836
g_fcn_ridft2
bool g_fcn_ridft2(double &obj, Vector3D g_obj[3], const Vector3D x[3], const Vector3D &n, const Matrix3D &invW, const double a, const Exponent b, const Exponent c, const double delta)
Definition: QualityMetric/DFT/RI_DFT.cpp:117
Mesquite::det
double det(const Matrix3D &A)
Definition: includeLinks/Matrix3D.hpp:549
Mesquite::PatchData::get_vertex_array
const MsqVertex * get_vertex_array(MsqError &err) const
Returns a pointer to the start of the vertex array.
Definition: includeLinks/PatchData.hpp:513
Mesquite::RI_DFT::invW
Matrix3D invW
Definition: includeLinks/RI_DFT.hpp:104
Mesquite::RI_DFT::evaluate_element
bool evaluate_element(PatchData &pd, MsqMeshEntity *e, double &m, MsqError &err)
Evaluate the metric for an element.
Definition: QualityMetric/DFT/RI_DFT.cpp:1812
j
j indices j
Definition: Indexing.h:6
Mesquite::PatchData::get_domain_normal_at_vertex
void get_domain_normal_at_vertex(size_t vertex_index, bool normalize, Vector3D &surf_norm, MsqError &err)
Definition: Mesh/PatchData.cpp:1117
Mesquite::TargetMatrix
Class containing the target corner matrices for the context based smoothing.
Definition: includeLinks/TargetMatrix.hpp:49
Mesquite::MsqMeshEntity::get_element_type
EntityTopology get_element_type() const
Returns element type.
Definition: includeLinks/MsqMeshEntity.hpp:78
Mesquite::MsqMeshEntity::get_vertex_index_array
const msq_stdc::size_t * get_vertex_index_array() const
Very efficient retrieval of vertices indexes (corresponding to the PatchData vertex array)...
Definition: includeLinks/MsqMeshEntity.hpp:196
Mesquite::inv
void inv(Matrix3D &Ainv, const Matrix3D &A)
Definition: includeLinks/Matrix3D.hpp:555
Mesquite::RI_DFT::mGrads
Vector3D mGrads[4]
Definition: includeLinks/RI_DFT.hpp:101
Mesquite::RI_DFT::mHessians
Matrix3D mHessians[10]
Definition: includeLinks/RI_DFT.hpp:103
Mesquite::DistanceFromTarget::get_barrier_function
bool get_barrier_function(PatchData &pd, const double &tau, double &h, MsqError &err)
Definition: includeLinks/DistanceFromTarget.hpp:98
h_fcn_ridft2
bool h_fcn_ridft2(double &obj, Vector3D g_obj[3], Matrix3D h_obj[6], const Vector3D x[3], const Vector3D &n, const Matrix3D &invW, const double a, const Exponent b, const Exponent c, const double delta)
Definition: QualityMetric/DFT/RI_DFT.cpp:263
Mesquite::MsqVertex
MsqVertex is the Mesquite object that stores information about the vertices in the mesh...
Definition: includeLinks/MsqVertex.hpp:53
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89
m_fcn_ridft2
bool m_fcn_ridft2(double &obj, const Vector3D x[3], const Vector3D &n, const Matrix3D &invW, const double a, const Exponent b, const Exponent c, const double delta)
Definition: QualityMetric/DFT/RI_DFT.cpp:59
h_fcn_ridft3
bool h_fcn_ridft3(double &obj, Vector3D g_obj[4], Matrix3D h_obj[10], const Vector3D x[4], const Matrix3D &invW, const double a, const Exponent b, const Exponent c, const double delta)
Definition: QualityMetric/DFT/RI_DFT.cpp:1081
Mesquite::RI_DFT::mAccGrads
Vector3D mAccGrads[8]
Definition: includeLinks/RI_DFT.hpp:102
Mesquite::MSQ_3RT_2_OVER_6RT_3
static const double MSQ_3RT_2_OVER_6RT_3
Definition: Mesquite.hpp:130
Mesquite::RI_DFT::mCoords
Vector3D mCoords[4]
Definition: includeLinks/RI_DFT.hpp:100
Mesquite::MSQ_MIN
const double MSQ_MIN
Definition: Mesquite.hpp:160
g_fcn_ridft3
bool g_fcn_ridft3(double &obj, Vector3D g_obj[4], const Vector3D x[4], const Matrix3D &invW, const double a, const Exponent b, const Exponent c, const double delta)
Definition: QualityMetric/DFT/RI_DFT.cpp:893