Rocstar  1.0
Rocstar multiphysics simulation application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Functions
QualityMetric/DFT/RI_DFT.cpp File Reference
#include <cstdio>
#include "RI_DFT.hpp"
Include dependency graph for QualityMetric/DFT/RI_DFT.cpp:

Go to the source code of this file.

Functions

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)
 
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)
 
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)
 
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)
 
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)
 
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)
 

Function Documentation

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 
)
inline

Definition at line 117 of file QualityMetric/DFT/RI_DFT.cpp.

References Mesquite::MSQ_MIN, Mesquite::pow(), and sqrt().

Referenced by RI_DFT::compute_element_analytical_gradient().

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 }
sqrt
double sqrt(double d)
Definition: double.h:73
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
inline

Definition at line 893 of file QualityMetric/DFT/RI_DFT.cpp.

References Mesquite::MSQ_MIN, Mesquite::pow(), and sqrt().

Referenced by RI_DFT::compute_element_analytical_gradient().

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 }
sqrt
double sqrt(double d)
Definition: double.h:73
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
inline

Definition at line 263 of file QualityMetric/DFT/RI_DFT.cpp.

References A, nvc::cross(), Matrix3D::fill_lower_triangle(), Mesquite::MSQ_MIN, Mesquite::pow(), and sqrt().

Referenced by RI_DFT::compute_element_analytical_hessian().

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 }
nvc::cross
NVec< 3, T > cross(const NVec< 3, T > &u, const NVec< 3, T > &v)
Definition: Rocon/include/NVec.h:185
sqrt
double sqrt(double d)
Definition: double.h:73
A
rational * A
Definition: vinci_lass.c:67
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
inline

Definition at line 1081 of file QualityMetric/DFT/RI_DFT.cpp.

References A, nvc::cross(), Matrix3D::fill_lower_triangle(), Mesquite::MSQ_MIN, Mesquite::pow(), and sqrt().

Referenced by RI_DFT::compute_element_analytical_hessian().

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 }
nvc::cross
NVec< 3, T > cross(const NVec< 3, T > &u, const NVec< 3, T > &v)
Definition: Rocon/include/NVec.h:185
sqrt
double sqrt(double d)
Definition: double.h:73
A
rational * A
Definition: vinci_lass.c:67
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
inline

Definition at line 59 of file QualityMetric/DFT/RI_DFT.cpp.

References Mesquite::MSQ_MIN, Mesquite::pow(), and sqrt().

Referenced by RI_DFT::evaluate_element().

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 }
sqrt
double sqrt(double d)
Definition: double.h:73
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
inline

Definition at line 836 of file QualityMetric/DFT/RI_DFT.cpp.

References Mesquite::MSQ_MIN, Mesquite::pow(), and sqrt().

Referenced by RI_DFT::evaluate_element().

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 }
sqrt
double sqrt(double d)
Definition: double.h:73
Mesquite::pow
double pow(double value, const Exponent &exp)
Definition: includeLinks/Exponent.hpp:89

Here is the call graph for this function:

Here is the caller graph for this function: