← Back to branch summary

~ubuntu-branches/debian/squeeze/gmsh/squeeze

« back to all changes in this revision

Viewing changes to Numeric/Numeric.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Christophe Prud'homme, Christophe Prud'homme
  • Date: 2009-07-13 15:49:21 UTC
  • mfrom: (7.1.4 sid)
  • Revision ID: james.westby@ubuntu.com-20090713154921-zer07j8wixwa07ig
Tags: 2.3.1.dfsg-4
http://bugs.debian.org/529972
[Christophe Prud'homme]
* Bug fix: "gmsh with cgns write support", thanks to Oliver Borm
  (Closes: #529972).
* debian/rules: make sure that Gmsh is built with occ support on all
  platforms thanks to Denis Barbier (#536435).

Show diffs side-by-side

added added

removed removed

Lines of Context:
Numeric/Numeric.cpp
7
7
#include "GmshMessage.h"
8
8
#include "Numeric.h"
9
9
 
10
 
#if defined(HAVE_GSL)
11
 
 
12
 
#include <gsl/gsl_errno.h>
13
 
#include <gsl/gsl_math.h>
14
 
#include <gsl/gsl_min.h>
15
 
#include <gsl/gsl_multimin.h>
16
 
#include <gsl/gsl_multiroots.h>
17
 
#include <gsl/gsl_linalg.h>
18
 
 
19
 
void my_gsl_msg(const char *reason, const char *file, int line,
20
 
                int gsl_errno)
21
 
{
22
 
  Msg::Error("GSL: %s (%s, line %d)", reason, file, line);
23
 
}
24
 
 
25
 
int Init_Numeric()
26
 
{
27
 
  // set new error handler
28
 
  gsl_set_error_handler(&my_gsl_msg);
29
 
 
30
 
  // initilize robust geometric predicates
31
 
  gmsh::exactinit() ;
32
 
  return 1;
33
 
}
34
 
 
35
 
static double (*f_statN) (double*, void *data); 
36
 
 
37
 
static void (*df_statN) (double*, double*, double &, void *data);
38
 
 
39
 
static double fobjN (const gsl_vector *x, void *data)
40
 
{
41
 
  return f_statN(x->data, data);
42
 
}
43
 
 
44
 
static void dfobjN(const gsl_vector *x, void *params, gsl_vector *g)
45
 
{
46
 
  double f;
47
 
  df_statN(x->data, g->data, f, params);
48
 
}
49
 
 
50
 
static void fdfobjN(const gsl_vector *x, void *params, double *f, gsl_vector *g)
51
 
{
52
 
  df_statN(x->data, g->data, *f, params);
53
 
}
54
 
 
55
 
void minimize_N(int N, double (*f)(double*, void *data), 
56
 
                void (*df)(double*, double*, double &, void *data),
57
 
                void *data, int niter, double *U, double &res)
58
 
{
59
 
  f_statN = f;
60
 
  df_statN = df;
61
 
 
62
 
  int iter = 0;
63
 
  int status;
64
 
  
65
 
  const gsl_multimin_fdfminimizer_type *T;
66
 
  gsl_multimin_fdfminimizer *s;
67
 
  gsl_vector *x;
68
 
  gsl_multimin_function_fdf my_func;
69
 
 
70
 
  my_func.f = &fobjN;
71
 
  my_func.df = &dfobjN;
72
 
  my_func.fdf = &fdfobjN;
73
 
  my_func.n = N;
74
 
  my_func.params = data;
75
 
 
76
 
  x = gsl_vector_alloc(N);
77
 
  for (int i = 0; i < N; i++)
78
 
    gsl_vector_set(x, i, U[i]);
79
 
 
80
 
  T = gsl_multimin_fdfminimizer_conjugate_fr;
81
 
  s = gsl_multimin_fdfminimizer_alloc(T, N);
82
 
 
83
 
  gsl_multimin_fdfminimizer_set(s, &my_func, x, 0.01, 1e-4);
84
 
 
85
 
  do{
86
 
    iter++;
87
 
    status = gsl_multimin_fdfminimizer_iterate(s);
88
 
    
89
 
    if (status)
90
 
      break;
91
 
    
92
 
    status = gsl_multimin_test_gradient(s->gradient, 1e-3);
93
 
  }
94
 
  while (status == GSL_CONTINUE && iter < niter);
95
 
  
96
 
  for (int i = 0; i < N; i++)
97
 
    U[i] = gsl_vector_get(s->x, i);
98
 
  res = s->f;
99
 
  gsl_multimin_fdfminimizer_free(s);
100
 
  gsl_vector_free(x);
101
 
}                                           
102
 
 
103
 
#else
104
 
 
105
 
int Init_Numeric()
106
 
{
107
 
  gmsh::exactinit() ;
108
 
  return 1;
109
 
}
110
 
 
111
 
void minimize_N(int N, double (*f) (double*, void *data), 
112
 
                void (*df) (double*, double*, double &, void *data) ,
113
 
                void *data,int niter,
114
 
                double *, double &res)
115
 
{
116
 
  Msg::Error("Gmsh must be compiled with GSL support for minimize_N");
117
 
}
118
 
 
119
 
#endif
 
10
#define SQU(a)      ((a)*(a))
 
11
 
 
12
double myatan2(double a, double b)
 
13
{
 
14
  if(a == 0.0 && b == 0)
 
15
    return 0.0;
 
16
  return atan2(a, b);
 
17
}
 
18
 
 
19
double myasin(double a)
 
20
{
 
21
  if(a <= -1.)
 
22
    return -M_PI / 2.;
 
23
  else if(a >= 1.)
 
24
    return M_PI / 2.;
 
25
  else
 
26
    return asin(a);
 
27
}
 
28
 
 
29
double myacos(double a)
 
30
{
 
31
  if(a <= -1.)
 
32
    return M_PI;
 
33
  else if(a >= 1.)
 
34
    return 0.;
 
35
  else
 
36
    return acos(a);
 
37
}
 
38
 
 
39
void matvec(double mat[3][3], double vec[3], double res[3])
 
40
{
 
41
  res[0] = mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2];
 
42
  res[1] = mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2];
 
43
  res[2] = mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2];
 
44
}
 
45
 
 
46
void normal3points(double x0, double y0, double z0,
 
47
                   double x1, double y1, double z1,
 
48
                   double x2, double y2, double z2,
 
49
                   double n[3])
 
50
{
 
51
  double t1[3] = {x1 - x0, y1 - y0, z1 - z0};
 
52
  double t2[3] = {x2 - x0, y2 - y0, z2 - z0};
 
53
  prodve(t1, t2, n);
 
54
  norme(n);
 
55
}
 
56
 
 
57
int sys2x2(double mat[2][2], double b[2], double res[2])
 
58
{
 
59
  double det, ud, norm;
 
60
  int i;
 
61
 
 
62
  norm = SQU(mat[0][0]) + SQU(mat[1][1]) + SQU(mat[0][1]) + SQU(mat[1][0]);
 
63
  det = mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1];
 
64
 
 
65
  // TOLERANCE ! WARNING WARNING
 
66
  if(norm == 0.0 || fabs(det) / norm < 1.e-12) {
 
67
    if(norm)
 
68
      Msg::Debug("Assuming 2x2 matrix is singular (det/norm == %.16g)",
 
69
          fabs(det) / norm);
 
70
    res[0] = res[1] = 0.0;
 
71
    return 0;
 
72
  }
 
73
  ud = 1. / det;
 
74
 
 
75
  res[0] = b[0] * mat[1][1] - mat[0][1] * b[1];
 
76
  res[1] = mat[0][0] * b[1] - mat[1][0] * b[0];
 
77
 
 
78
  for(i = 0; i < 2; i++)
 
79
    res[i] *= ud;
 
80
 
 
81
  return (1);
 
82
}
 
83
 
 
84
double det3x3(double mat[3][3])
 
85
{
 
86
  return (mat[0][0] * (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) -
 
87
          mat[0][1] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) +
 
88
          mat[0][2] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]));
 
89
}
 
90
 
 
91
double trace3x3(double mat[3][3])
 
92
{
 
93
  return mat[0][0] + mat[1][1] + mat[2][2];
 
94
}
 
95
 
 
96
double trace2 (double mat[3][3])
 
97
{
 
98
  double a00 =  mat[0][0] * mat[0][0] + mat[1][0] * mat[0][1] + mat[2][0] * mat[0][2]; 
 
99
  double a11 =  mat[1][0] * mat[0][1] + mat[1][1] * mat[1][1] + mat[1][2] * mat[2][1]; 
 
100
  double a22 =  mat[2][0] * mat[0][2] + mat[2][1] * mat[1][2] + mat[2][2] * mat[2][2];
 
101
  
 
102
  return a00 + a11 + a22;
 
103
}
 
104
 
 
105
int sys3x3(double mat[3][3], double b[3], double res[3], double *det)
 
106
{
 
107
  double ud;
 
108
  int i;
 
109
 
 
110
  *det = det3x3(mat);
 
111
 
 
112
  if(*det == 0.0) {
 
113
    res[0] = res[1] = res[2] = 0.0;
 
114
    return (0);
 
115
  }
 
116
 
 
117
  ud = 1. / (*det);
 
118
 
 
119
  res[0] = b[0] * (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) -
 
120
    mat[0][1] * (b[1] * mat[2][2] - mat[1][2] * b[2]) +
 
121
    mat[0][2] * (b[1] * mat[2][1] - mat[1][1] * b[2]);
 
122
 
 
123
  res[1] = mat[0][0] * (b[1] * mat[2][2] - mat[1][2] * b[2]) -
 
124
    b[0] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) +
 
125
    mat[0][2] * (mat[1][0] * b[2] - b[1] * mat[2][0]);
 
126
 
 
127
  res[2] = mat[0][0] * (mat[1][1] * b[2] - b[1] * mat[2][1]) -
 
128
    mat[0][1] * (mat[1][0] * b[2] - b[1] * mat[2][0]) +
 
129
    b[0] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]);
 
130
 
 
131
  for(i = 0; i < 3; i++)
 
132
    res[i] *= ud;
 
133
  return (1);
 
134
}
 
135
 
 
136
int sys3x3_with_tol(double mat[3][3], double b[3], double res[3], double *det)
 
137
{
 
138
  int out;
 
139
  double norm;
 
140
 
 
141
  out = sys3x3(mat, b, res, det);
 
142
  norm =
 
143
    SQU(mat[0][0]) + SQU(mat[0][1]) + SQU(mat[0][2]) +
 
144
    SQU(mat[1][0]) + SQU(mat[1][1]) + SQU(mat[1][2]) +
 
145
    SQU(mat[2][0]) + SQU(mat[2][1]) + SQU(mat[2][2]);
 
146
 
 
147
  // TOLERANCE ! WARNING WARNING
 
148
  if(norm == 0.0 || fabs(*det) / norm < 1.e-12) {
 
149
    if(norm)
 
150
      Msg::Debug("Assuming 3x3 matrix is singular (det/norm == %.16g)",
 
151
          fabs(*det) / norm);
 
152
    res[0] = res[1] = res[2] = 0.0;
 
153
    return 0;
 
154
  }
 
155
 
 
156
  return out;
 
157
}
 
158
 
 
159
double det2x2(double mat[2][2])
 
160
{
 
161
  return mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1];
 
162
}
 
163
 
 
164
double det2x3(double mat[2][3])
 
165
{
 
166
  double v1[3] = {mat[0][0], mat[0][1], mat[0][2]};
 
167
  double v2[3] = {mat[1][0], mat[1][1], mat[1][2]};
 
168
  double n[3];
 
169
  
 
170
  prodve(v1, v2, n);
 
171
  return norm3(n);
 
172
}
 
173
 
 
174
double inv2x2(double mat[2][2], double inv[2][2])
 
175
{
 
176
  const double det = det2x2(mat);
 
177
  if(det){
 
178
    double ud = 1. / det;
 
179
    inv[0][0] =  mat[1][1] * ud;
 
180
    inv[1][0] = -mat[1][0] * ud;
 
181
    inv[0][1] = -mat[0][1] * ud;
 
182
    inv[1][1] =  mat[0][0] * ud;
 
183
  }
 
184
  else{
 
185
    Msg::Error("Singular matrix");
 
186
    for(int i = 0; i < 2; i++)
 
187
      for(int j = 0; j < 2; j++)
 
188
        inv[i][j] = 0.;
 
189
  }
 
190
  return det;
 
191
}
 
192
 
 
193
double inv3x3(double mat[3][3], double inv[3][3])
 
194
{
 
195
  double det = det3x3(mat);
 
196
  if(det){
 
197
    double ud = 1. / det;
 
198
    inv[0][0] =  (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) * ud;
 
199
    inv[1][0] = -(mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) * ud;
 
200
    inv[2][0] =  (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]) * ud;
 
201
    inv[0][1] = -(mat[0][1] * mat[2][2] - mat[0][2] * mat[2][1]) * ud;
 
202
    inv[1][1] =  (mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]) * ud;
 
203
    inv[2][1] = -(mat[0][0] * mat[2][1] - mat[0][1] * mat[2][0]) * ud;
 
204
    inv[0][2] =  (mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]) * ud;
 
205
    inv[1][2] = -(mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0]) * ud;
 
206
    inv[2][2] =  (mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]) * ud;
 
207
  }
 
208
  else{
 
209
    Msg::Error("Singular matrix");
 
210
    for(int i = 0; i < 3; i++)
 
211
      for(int j = 0; j < 3; j++)
 
212
        inv[i][j] = 0.;
 
213
  }
 
214
  return det;
 
215
}
 
216
 
 
217
double angle_02pi(double A3)
 
218
{
 
219
  double DP = 2 * M_PI;
 
220
  while(A3 > DP || A3 < 0.) {
 
221
    if(A3 > 0)
 
222
      A3 -= DP;
 
223
    else
 
224
      A3 += DP;
 
225
  }
 
226
  return A3;
 
227
}
 
228
 
 
229
double angle_plan(double V[3], double P1[3], double P2[3], double n[3])
 
230
{
 
231
  double PA[3], PB[3], angplan;
 
232
  double cosc, sinc, c[3];
 
233
 
 
234
  PA[0] = P1[0] - V[0];
 
235
  PA[1] = P1[1] - V[1];
 
236
  PA[2] = P1[2] - V[2];
 
237
 
 
238
  PB[0] = P2[0] - V[0];
 
239
  PB[1] = P2[1] - V[1];
 
240
  PB[2] = P2[2] - V[2];
 
241
 
 
242
  norme(PA);
 
243
  norme(PB);
 
244
 
 
245
  prodve(PA, PB, c);
 
246
 
 
247
  prosca(PA, PB, &cosc);
 
248
  prosca(c, n, &sinc);
 
249
  angplan = myatan2(sinc, cosc);
 
250
 
 
251
  return angplan;
 
252
}
 
253
 
 
254
double triangle_area(double p0[3], double p1[3], double p2[3])
 
255
{
 
256
  double a[3], b[3], c[3];
 
257
  
 
258
  a[0] = p2[0] - p1[0];
 
259
  a[1] = p2[1] - p1[1];
 
260
  a[2] = p2[2] - p1[2];
 
261
  
 
262
  b[0] = p0[0] - p1[0];
 
263
  b[1] = p0[1] - p1[1];
 
264
  b[2] = p0[2] - p1[2];
 
265
  
 
266
  prodve(a, b, c);
 
267
  return 0.5 * sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]);
 
268
}
 
269
 
 
270
char float2char(float f)
 
271
{
 
272
  // float normalized in [-1, 1], char in [-127, 127]
 
273
  f *= 127.;
 
274
  if(f > 127.) return 127;
 
275
  else if(f < -127.) return -127;
 
276
  else return (char)f;
 
277
}
 
278
 
 
279
float char2float(char c)
 
280
{
 
281
  float f = c;
 
282
  f /= 127.;
 
283
  if(f > 1.) return 1.;
 
284
  else if(f < -1) return -1.;
 
285
  else return f;
 
286
}
 
287
 
 
288
double InterpolateIso(double *X, double *Y, double *Z,
 
289
                      double *Val, double V, int I1, int I2,
 
290
                      double *XI, double *YI, double *ZI)
 
291
{
 
292
  if(Val[I1] == Val[I2]) {
 
293
    *XI = X[I1];
 
294
    *YI = Y[I1];
 
295
    *ZI = Z[I1];
 
296
    return 0;
 
297
  }
 
298
  else {
 
299
    double coef = (V - Val[I1]) / (Val[I2] - Val[I1]);
 
300
    *XI = coef * (X[I2] - X[I1]) + X[I1];
 
301
    *YI = coef * (Y[I2] - Y[I1]) + Y[I1];
 
302
    *ZI = coef * (Z[I2] - Z[I1]) + Z[I1];
 
303
    return coef;
 
304
  }
 
305
}
 
306
 
 
307
void gradSimplex(double *x, double *y, double *z, double *v, double *grad)
 
308
{
 
309
  // p = p1 * (1-u-v-w) + p2 u + p3 v + p4 w
 
310
 
 
311
  double mat[3][3];
 
312
  double det, b[3];
 
313
  mat[0][0] = x[1] - x[0];
 
314
  mat[1][0] = x[2] - x[0];
 
315
  mat[2][0] = x[3] - x[0];
 
316
  mat[0][1] = y[1] - y[0];
 
317
  mat[1][1] = y[2] - y[0];
 
318
  mat[2][1] = y[3] - y[0];
 
319
  mat[0][2] = z[1] - z[0];
 
320
  mat[1][2] = z[2] - z[0];
 
321
  mat[2][2] = z[3] - z[0];
 
322
  b[0] = v[1] - v[0];
 
323
  b[1] = v[2] - v[0];
 
324
  b[2] = v[3] - v[0];
 
325
  sys3x3(mat, b, grad, &det);
 
326
}
 
327
 
 
328
double ComputeVonMises(double *V)
 
329
{
 
330
  double tr = (V[0] + V[4] + V[8]) / 3.;
 
331
  double v11 = V[0] - tr, v12 = V[1],      v13 = V[2];
 
332
  double v21 = V[3],      v22 = V[4] - tr, v23 = V[5];
 
333
  double v31 = V[6],      v32 = V[7],      v33 = V[8] - tr;
 
334
  return sqrt(1.5 * (v11 * v11 + v12 * v12 + v13 * v13 +
 
335
                     v21 * v21 + v22 * v22 + v23 * v23 +
 
336
                     v31 * v31 + v32 * v32 + v33 * v33));
 
337
}
 
338
 
 
339
double ComputeScalarRep(int numComp, double *val)
 
340
{
 
341
  if(numComp == 1)
 
342
    return val[0];
 
343
  else if(numComp == 3)
 
344
    return sqrt(val[0] * val[0] + val[1] * val[1] + val[2] * val[2]);
 
345
  else if(numComp == 9)
 
346
    return ComputeVonMises(val);
 
347
  return 0.;
 
348
}
 
349
 
 
350
void eigenvalue(double mat[3][3], double v[3])
 
351
{            
 
352
  // characteristic polynomial of T : find v root of
 
353
  // v^3 - I1 v^2 + I2 T - I3 = 0
 
354
  // I1 : first invariant , trace(T)
 
355
  // I2 : second invariant , 1/2 (I1^2 -trace(T^2))
 
356
  // I3 : third invariant , det T
 
357
 
 
358
  double c[4];
 
359
  c[3] = 1.0;
 
360
  c[2] = - trace3x3(mat);
 
361
  c[1] = 0.5 * (c[2] * c[2] - trace2(mat));
 
362
  c[0] = - det3x3(mat);
 
363
  
 
364
  //printf("%g %g %g\n", mat[0][0], mat[0][1], mat[0][2]);
 
365
  //printf("%g %g %g\n", mat[1][0], mat[1][1], mat[1][2]);
 
366
  //printf("%g %g %g\n", mat[2][0], mat[2][1], mat[2][2]);
 
367
  //printf("%g x^3 + %g x^2 + %g x + %g = 0\n", c[3], c[2], c[1], c[0]);
 
368
  
 
369
  double imag[3];
 
370
  FindCubicRoots(c, v, imag);
 
371
  eigsort(v);
 
372
}
 
373
 
 
374
void FindCubicRoots(const double coef[4], double real[3], double imag[3])
 
375
{
 
376
  double a = coef[3];
 
377
  double b = coef[2];
 
378
  double c = coef[1];
 
379
  double d = coef[0];
 
380
 
 
381
  if(!a || !d){
 
382
    Msg::Error("Degenerate cubic: use a second degree solver!");
 
383
    return;
 
384
  }
 
385
 
 
386
  b /= a;
 
387
  c /= a;
 
388
  d /= a;
 
389
  
 
390
  double q = (3.0*c - (b*b))/9.0;
 
391
  double r = -(27.0*d) + b*(9.0*c - 2.0*(b*b));
 
392
  r /= 54.0;
 
393
 
 
394
  double discrim = q*q*q + r*r;
 
395
  imag[0] = 0.0; // The first root is always real.
 
396
  double term1 = (b/3.0);
 
397
 
 
398
  if (discrim > 0) { // one root is real, two are complex
 
399
    double s = r + sqrt(discrim);
 
400
    s = ((s < 0) ? -pow(-s, (1.0/3.0)) : pow(s, (1.0/3.0)));
 
401
    double t = r - sqrt(discrim);
 
402
    t = ((t < 0) ? -pow(-t, (1.0/3.0)) : pow(t, (1.0/3.0)));
 
403
    real[0] = -term1 + s + t;
 
404
    term1 += (s + t)/2.0;
 
405
    real[1] = real[2] = -term1;
 
406
    term1 = sqrt(3.0)*(-t + s)/2;
 
407
    imag[1] = term1;
 
408
    imag[2] = -term1;
 
409
    return;
 
410
  }
 
411
 
 
412
  // The remaining options are all real
 
413
  imag[1] = imag[2] = 0.0;
 
414
  
 
415
  double r13;
 
416
  if (discrim == 0){ // All roots real, at least two are equal.
 
417
    r13 = ((r < 0) ? -pow(-r,(1.0/3.0)) : pow(r,(1.0/3.0)));
 
418
    real[0] = -term1 + 2.0*r13;
 
419
    real[1] = real[2] = -(r13 + term1);
 
420
    return;
 
421
  }
 
422
 
 
423
  // Only option left is that all roots are real and unequal (to get
 
424
  // here, q < 0)
 
425
  q = -q;
 
426
  double dum1 = q*q*q;
 
427
  dum1 = acos(r/sqrt(dum1));
 
428
  r13 = 2.0*sqrt(q);
 
429
  real[0] = -term1 + r13*cos(dum1/3.0);
 
430
  real[1] = -term1 + r13*cos((dum1 + 2.0*M_PI)/3.0);
 
431
  real[2] = -term1 + r13*cos((dum1 + 4.0*M_PI)/3.0);
 
432
}
 
433
 
 
434
void eigsort(double d[3])
 
435
{
 
436
  int k, j, i;
 
437
  double p;
 
438
  
 
439
  for (i=0; i<3; i++) {
 
440
    p=d[k=i];
 
441
    for (j=i+1;j<3;j++)
 
442
      if (d[j] >= p) p=d[k=j];
 
443
    if (k != i) {
 
444
      d[k]=d[i];
 
445
      d[i]=p;
 
446
    }
 
447
  }
 
448
}
 
449
 
 
450
void invert_singular_matrix3x3(double MM[3][3], double II[3][3])
 
451
{
 
452
  int i, j, k, n = 3;
 
453
  double TT[3][3];
 
454
 
 
455
  for(i = 1; i <= n; i++) {
 
456
    for(j = 1; j <= n; j++) {
 
457
      II[i - 1][j - 1] = 0.0;
 
458
      TT[i - 1][j - 1] = 0.0;
 
459
    }
 
460
  }
 
461
 
 
462
  gmshMatrix<double> M(3, 3), V(3, 3);
 
463
  gmshVector<double> W(3);
 
464
  for(i = 1; i <= n; i++){
 
465
    for(j = 1; j <= n; j++){
 
466
      M(i - 1, j - 1) = MM[i - 1][j - 1];
 
467
    }
 
468
  }
 
469
  M.svd(V, W);
 
470
  for(i = 1; i <= n; i++) {
 
471
    for(j = 1; j <= n; j++) {
 
472
      double ww = W(i - 1);
 
473
      if(fabs(ww) > 1.e-16) { // singular value precision
 
474
        TT[i - 1][j - 1] += M(j - 1, i - 1) / ww;
 
475
      }
 
476
    }
 
477
  }
 
478
  for(i = 1; i <= n; i++) {
 
479
    for(j = 1; j <= n; j++) {
 
480
      for(k = 1; k <= n; k++) {
 
481
        II[i - 1][j - 1] += V(i - 1, k - 1) * TT[k - 1][j - 1];
 
482
      }
 
483
    }
 
484
  }
 
485
}
 
486
 
 
487
bool newton_fd(void (*func)(gmshVector<double> &, gmshVector<double> &, void *),
 
488
               gmshVector<double> &x, void *data, double relax, double tolx)
 
489
{
 
490
  const int MAXIT = 50;
 
491
  const double EPS = 1.e-4;
 
492
  const int N = x.size();
 
493
  
 
494
  gmshMatrix<double> J(N, N);
 
495
  gmshVector<double> f(N), feps(N), dx(N);
 
496
  
 
497
  for (int iter = 0; iter < MAXIT; iter++){
 
498
    func(x, f, data);
 
499
 
 
500
    for (int j = 0; j < N; j++){
 
501
      double h = EPS * fabs(x(j));
 
502
      if(h == 0.) h = EPS;
 
503
      x(j) += h;
 
504
      func(x, feps, data);
 
505
      for (int i = 0; i < N; i++)
 
506
        J(i, j) = (feps(i) - f(i)) / h;
 
507
      x(j) -= h;
 
508
    }
 
509
    
 
510
    if (N == 1)
 
511
      dx(0) = f(0) / J(0, 0);
 
512
    else
 
513
      J.lu_solve(f, dx);
 
514
    
 
515
    for (int i = 0; i < N; i++)
 
516
      x(i) -= relax * dx(i);
 
517
 
 
518
    if(dx.norm() < tolx) return true; 
 
519
  }
 
520
  return false;
 
521
}