← Back to branch summary

~ubuntu-branches/ubuntu/karmic/gmsh/karmic

« 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, Daniel Leidert
  • Date: 2008-05-18 12:46:05 UTC
  • mfrom: (1.2.6 upstream)
  • Revision ID: james.westby@ubuntu.com-20080518124605-716xqbqeo07o497k
Tags: 2.2.0-2
[Christophe Prud'homme]
* Bug fix: "gmsh ships no .desktop", thanks to Vassilis Pandis (Closes:
  #375770). Applied the Ubuntu patch.

[Daniel Leidert]
* debian/control (Vcs-Svn): Fixed.
  (Build-Depends): Use texlive instead of tetex-bin.
* debian/gmsh.doc-base (Section): Fixed accordingly to doc-base (>= 0.8.10).
* debian/rules: Removed some variable declarations, that lead to double
  configuration and seem to be useless.
  (build/gmsh): Try to avoid multiple runs by using a stamp.
  (orig-tarball): Renamed to get-orig-source and changed to use uscan.
* debian/watch: Added.

Show diffs side-by-side

added added

removed removed

Lines of Context:
Numeric/Numeric.cpp
1
 
// $Id: Numeric.cpp,v 1.31 2007-01-12 13:16:59 remacle Exp $
 
1
// $Id: Numeric.cpp,v 1.40 2008-02-17 08:48:02 geuzaine Exp $
2
2
//
3
 
// Copyright (C) 1997-2007 C. Geuzaine, J.-F. Remacle
 
3
// Copyright (C) 1997-2008 C. Geuzaine, J.-F. Remacle
4
4
//
5
5
// This program is free software; you can redistribute it and/or modify
6
6
// it under the terms of the GNU General Public License as published by
19
19
// 
20
20
// Please report all bugs and problems to <gmsh@geuz.org>.
21
21
 
22
 
// this file should contain only purely numerical routines (that do
23
 
// not depend on any Gmsh structures)
24
 
 
25
 
#include "Gmsh.h"
 
22
#include "Message.h"
26
23
#include "Numeric.h"
27
24
 
28
25
// Check GSL version. We need at least 1.2, since all versions <=
80
77
 
81
78
#endif
82
79
 
83
 
double myatan2(double a, double b)
84
 
{
85
 
  if(a == 0.0 && b == 0)
86
 
    return 0.0;
87
 
  return atan2(a, b);
88
 
}
89
 
 
90
 
double myasin(double a)
91
 
{
92
 
  if(a <= -1.)
93
 
    return -Pi / 2.;
94
 
  else if(a >= 1.)
95
 
    return Pi / 2.;
96
 
  else
97
 
    return asin(a);
98
 
}
99
 
 
100
 
double myacos(double a)
101
 
{
102
 
  if(a <= -1.)
103
 
    return Pi;
104
 
  else if(a >= 1.)
105
 
    return 0.;
106
 
  else
107
 
    return acos(a);
108
 
}
109
 
 
110
 
void prodve(double a[3], double b[3], double c[3])
111
 
{
112
 
  c[2] = a[0] * b[1] - a[1] * b[0];
113
 
  c[1] = -a[0] * b[2] + a[2] * b[0];
114
 
  c[0] = a[1] * b[2] - a[2] * b[1];
115
 
}
116
 
 
117
 
void matvec(double mat[3][3], double vec[3], double res[3])
118
 
{
119
 
  res[0] = mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2];
120
 
  res[1] = mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2];
121
 
  res[2] = mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2];
122
 
}
123
 
 
124
 
void prosca(double a[3], double b[3], double *c)
125
 
{
126
 
  *c = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
127
 
}
128
 
 
129
 
double norm3(double a[3])
130
 
{
131
 
  return sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
132
 
}
133
 
 
134
 
double norme(double a[3])
135
 
{
136
 
  double mod = norm3(a);
137
 
  if(mod != 0.0){
138
 
    a[0] /= mod;
139
 
    a[1] /= mod;
140
 
    a[2] /= mod;
141
 
  }
142
 
  return mod;
143
 
}
144
 
 
145
 
void normal3points(double x0, double y0, double z0,
146
 
                   double x1, double y1, double z1,
147
 
                   double x2, double y2, double z2,
148
 
                   double n[3])
149
 
{
150
 
  double t1[3] = {x1 - x0, y1 - y0, z1 - z0};
151
 
  double t2[3] = {x2 - x0, y2 - y0, z2 - z0};
152
 
  prodve(t1, t2, n);
153
 
  norme(n);
154
 
}
155
 
 
156
 
int sys2x2(double mat[2][2], double b[2], double res[2])
157
 
{
158
 
  double det, ud, norm;
159
 
  int i;
160
 
 
161
 
  norm = DSQR(mat[0][0]) + DSQR(mat[1][1]) + DSQR(mat[0][1]) + DSQR(mat[1][0]);
162
 
  det = mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1];
163
 
 
164
 
 
165
 
  // TOLERANCE ! WARNING WARNING
166
 
  if(norm == 0.0 || fabs(det) / norm < 1.e-12) {
167
 
    if(norm)
168
 
      Msg(DEBUG, "Assuming 2x2 matrix is singular (det/norm == %.16g)",
169
 
          fabs(det) / norm);
170
 
    res[0] = res[1] = 0.0;
171
 
    return 0;
172
 
  }
173
 
  ud = 1. / det;
174
 
 
175
 
  res[0] = b[0] * mat[1][1] - mat[0][1] * b[1];
176
 
  res[1] = mat[0][0] * b[1] - mat[1][0] * b[0];
177
 
 
178
 
  for(i = 0; i < 2; i++)
179
 
    res[i] *= ud;
180
 
 
181
 
  return (1);
182
 
}
183
 
 
184
 
double det3x3(double mat[3][3])
185
 
{
186
 
  return (mat[0][0] * (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) -
187
 
          mat[0][1] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) +
188
 
          mat[0][2] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]));
189
 
}
190
 
 
191
 
double trace3x3(double mat[3][3])
192
 
{
193
 
  return mat[0][0] + mat[1][1] + mat[2][2];
194
 
}
195
 
 
196
 
double trace2 (double mat[3][3])
197
 
{
198
 
  double a00 =  mat[0][0] * mat[0][0] + mat[1][0] * mat[0][1] + mat[2][0] * mat[0][2]; 
199
 
  double a11 =  mat[1][0] * mat[0][1] + mat[1][1] * mat[1][1] + mat[1][2] * mat[2][1]; 
200
 
  double a22 =  mat[2][0] * mat[0][2] + mat[2][1] * mat[1][2] + mat[2][2] * mat[2][2];
201
 
  
202
 
  return a00 + a11 + a22;
203
 
}
204
 
 
205
 
int sys3x3(double mat[3][3], double b[3], double res[3], double *det)
206
 
{
207
 
  double ud;
208
 
  int i;
209
 
 
210
 
  *det = det3x3(mat);
211
 
 
212
 
  if(*det == 0.0) {
213
 
    res[0] = res[1] = res[2] = 0.0;
214
 
    return (0);
215
 
  }
216
 
 
217
 
  ud = 1. / (*det);
218
 
 
219
 
  res[0] = b[0] * (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) -
220
 
    mat[0][1] * (b[1] * mat[2][2] - mat[1][2] * b[2]) +
221
 
    mat[0][2] * (b[1] * mat[2][1] - mat[1][1] * b[2]);
222
 
 
223
 
  res[1] = mat[0][0] * (b[1] * mat[2][2] - mat[1][2] * b[2]) -
224
 
    b[0] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) +
225
 
    mat[0][2] * (mat[1][0] * b[2] - b[1] * mat[2][0]);
226
 
 
227
 
  res[2] = mat[0][0] * (mat[1][1] * b[2] - b[1] * mat[2][1]) -
228
 
    mat[0][1] * (mat[1][0] * b[2] - b[1] * mat[2][0]) +
229
 
    b[0] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]);
230
 
 
231
 
  for(i = 0; i < 3; i++)
232
 
    res[i] *= ud;
233
 
  return (1);
234
 
 
235
 
}
236
 
 
237
 
int sys3x3_with_tol(double mat[3][3], double b[3], double res[3], double *det)
238
 
{
239
 
  int out;
240
 
  double norm;
241
 
 
242
 
  out = sys3x3(mat, b, res, det);
243
 
  norm =
244
 
    DSQR(mat[0][0]) + DSQR(mat[0][1]) + DSQR(mat[0][2]) +
245
 
    DSQR(mat[1][0]) + DSQR(mat[1][1]) + DSQR(mat[1][2]) +
246
 
    DSQR(mat[2][0]) + DSQR(mat[2][1]) + DSQR(mat[2][2]);
247
 
 
248
 
  // TOLERANCE ! WARNING WARNING
249
 
  if(norm == 0.0 || fabs(*det) / norm < 1.e-12) {
250
 
    if(norm)
251
 
      Msg(DEBUG, "Assuming 3x3 matrix is singular (det/norm == %.16g)",
252
 
          fabs(*det) / norm);
253
 
    res[0] = res[1] = res[2] = 0.0;
254
 
    return 0;
255
 
  }
256
 
 
257
 
  return out;
258
 
}
259
 
 
260
 
double det2x2(double mat[2][2])
261
 
{
262
 
  return mat[0][0] *mat[1][1] -mat[1][0] *mat[0][1];
263
 
}
264
 
 
265
 
double inv2x2(double mat[2][2], double inv[2][2])
266
 
{
267
 
  const double det = det2x2 ( mat );
268
 
  if(det){
269
 
    double ud = 1. / det;
270
 
    inv[0][0] =  ( mat[1][1] ) * ud ;
271
 
    inv[1][0] = -( mat[1][0] ) * ud ;
272
 
    inv[0][1] = -( mat[0][1] ) * ud ;
273
 
    inv[1][1] =  ( mat[0][0] ) * ud ;
274
 
  }
275
 
  else{
276
 
    Msg(GERROR, "Singular matrix");
277
 
    for(int i = 0; i < 2; i++)
278
 
      for(int j = 0; j < 2; j++)
279
 
        inv[i][j] = 0.;
280
 
  }
281
 
  return det;
282
 
}
283
 
 
284
 
double inv3x3(double mat[3][3], double inv[3][3])
285
 
{
286
 
  double det = det3x3(mat);
287
 
  if(det){
288
 
    double ud = 1. / det;
289
 
    inv[0][0] =  ( mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1] ) * ud ;
290
 
    inv[1][0] = -( mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0] ) * ud ;
291
 
    inv[2][0] =  ( mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0] ) * ud ;
292
 
    inv[0][1] = -( mat[0][1] * mat[2][2] - mat[0][2] * mat[2][1] ) * ud ;
293
 
    inv[1][1] =  ( mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0] ) * ud ;
294
 
    inv[2][1] = -( mat[0][0] * mat[2][1] - mat[0][1] * mat[2][0] ) * ud ;
295
 
    inv[0][2] =  ( mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1] ) * ud ;
296
 
    inv[1][2] = -( mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0] ) * ud ;
297
 
    inv[2][2] =  ( mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0] ) * ud ;
298
 
  }
299
 
  else{
300
 
    Msg(GERROR, "Singular matrix");
301
 
    for(int i = 0; i < 3; i++)
302
 
      for(int j = 0; j < 3; j++)
303
 
        inv[i][j] = 0.;
304
 
  }
305
 
  return det;
306
 
}
307
 
 
308
80
void invert_singular_matrix3x3(double MM[3][3], double II[3][3])
309
81
{
310
82
  int i, j, k, n = 3;
377
149
  free_dvector(W, 1, n);
378
150
#endif
379
151
}
380
 
 
381
 
double angle_02pi(double A3)
382
 
{
383
 
  double DP = 2 * Pi;
384
 
  while(A3 > DP || A3 < 0.) {
385
 
    if(A3 > 0)
386
 
      A3 -= DP;
387
 
    else
388
 
      A3 += DP;
389
 
  }
390
 
  return A3;
391
 
}
392
 
 
393
 
double angle_plan(double V[3], double P1[3], double P2[3], double n[3])
394
 
{
395
 
  double PA[3], PB[3], angplan;
396
 
  double cosc, sinc, c[3];
397
 
 
398
 
  PA[0] = P1[0] - V[0];
399
 
  PA[1] = P1[1] - V[1];
400
 
  PA[2] = P1[2] - V[2];
401
 
 
402
 
  PB[0] = P2[0] - V[0];
403
 
  PB[1] = P2[1] - V[1];
404
 
  PB[2] = P2[2] - V[2];
405
 
 
406
 
  norme(PA);
407
 
  norme(PB);
408
 
 
409
 
  prodve(PA, PB, c);
410
 
 
411
 
  prosca(PA, PB, &cosc);
412
 
  prosca(c, n, &sinc);
413
 
  angplan = myatan2(sinc, cosc);
414
 
 
415
 
  return angplan;
416
 
}
417
 
 
418
 
double triangle_area(double p0[3], double p1[3], double p2[3])
419
 
{
420
 
  double a[3], b[3], c[3];
421
 
  
422
 
  a[0] = p2[0] - p1[0];
423
 
  a[1] = p2[1] - p1[1];
424
 
  a[2] = p2[2] - p1[2];
425
 
  
426
 
  b[0] = p0[0] - p1[0];
427
 
  b[1] = p0[1] - p1[1];
428
 
  b[2] = p0[2] - p1[2];
429
 
  
430
 
  prodve(a, b, c);
431
 
  return (0.5 * sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]));
432
 
}
433
 
 
434
 
char float2char(float f)
435
 
{
436
 
  // f is supposed to be normalized in [-1, 1]
437
 
  f = (f > 1.) ? 1. : (f < -1.) ? -1 : f;
438
 
  // char is in [-128, 127]
439
 
  return (char)(-128 + (f + 1.)/2. * 255);
440
 
}
441
 
 
442
 
float char2float(char c)
443
 
{
444
 
  return -1. + (float)(c + 128)/255. * 2.;
445
 
}
446
 
 
447
 
double InterpolateIso(double *X, double *Y, double *Z,
448
 
                      double *Val, double V, int I1, int I2,
449
 
                      double *XI, double *YI, double *ZI)
450
 
{
451
 
  if(Val[I1] == Val[I2]) {
452
 
    *XI = X[I1];
453
 
    *YI = Y[I1];
454
 
    *ZI = Z[I1];
455
 
    return 0;
456
 
  }
457
 
  else {
458
 
    double coef = (V - Val[I1]) / (Val[I2] - Val[I1]);
459
 
    *XI = coef * (X[I2] - X[I1]) + X[I1];
460
 
    *YI = coef * (Y[I2] - Y[I1]) + Y[I1];
461
 
    *ZI = coef * (Z[I2] - Z[I1]) + Z[I1];
462
 
    return coef;
463
 
  }
464
 
}
465
 
 
466
 
void gradSimplex(double *x, double *y, double *z, double *v, double *grad)
467
 
{
468
 
  // p = p1 * (1-u-v-w) + p2 u + p3 v + p4 w
469
 
 
470
 
  double mat[3][3];
471
 
  double det, b[3];
472
 
  mat[0][0] = x[1] - x[0];
473
 
  mat[1][0] = x[2] - x[0];
474
 
  mat[2][0] = x[3] - x[0];
475
 
  mat[0][1] = y[1] - y[0];
476
 
  mat[1][1] = y[2] - y[0];
477
 
  mat[2][1] = y[3] - y[0];
478
 
  mat[0][2] = z[1] - z[0];
479
 
  mat[1][2] = z[2] - z[0];
480
 
  mat[2][2] = z[3] - z[0];
481
 
  b[0] = v[1] - v[0];
482
 
  b[1] = v[2] - v[0];
483
 
  b[2] = v[3] - v[0];
484
 
  sys3x3(mat, b, grad, &det);
485
 
}
486
 
 
487
 
void eigenvalue(double mat[3][3], double v[3])
488
 
{            
489
 
  // characteristic polynomial of T : find v root of
490
 
  // v^3 - I1 v^2 + I2 T - I3 = 0
491
 
  // I1 : first invariant , trace(T)
492
 
  // I2 : second invariant , 1/2 (I1^2 -trace(T^2))
493
 
  // I3 : third invariant , det T
494
 
 
495
 
  double c[4];
496
 
  c[3] = 1.0;
497
 
  c[2] = - trace3x3(mat);
498
 
  c[1] = 0.5 * (c[2]*c[2] - trace2(mat));
499
 
  c[0] = - det3x3(mat);
500
 
  
501
 
  //printf("%g %g %g\n", mat[0][0], mat[0][1], mat[0][2]);
502
 
  //printf("%g %g %g\n", mat[1][0], mat[1][1], mat[1][2]);
503
 
  //printf("%g %g %g\n", mat[2][0], mat[2][1], mat[2][2]);
504
 
  //printf("%g x^3 + %g x^2 + %g x + %g = 0\n", c[3], c[2], c[1], c[0]);
505
 
  
506
 
  double imag[3];
507
 
  FindCubicRoots(c, v, imag);
508
 
  eigsort(v);
509
 
}
510
 
 
511
 
void FindCubicRoots(const double coef[4], double real[3], double imag[3])
512
 
{
513
 
  double a = coef[3];
514
 
  double b = coef[2];
515
 
  double c = coef[1];
516
 
  double d = coef[0];
517
 
 
518
 
  if(!a || !d){
519
 
    Msg(GERROR, "Degenerate cubic: use a second degree solver!");
520
 
    return;
521
 
  }
522
 
 
523
 
  b /= a;
524
 
  c /= a;
525
 
  d /= a;
526
 
  
527
 
  double q = (3.0*c - (b*b))/9.0;
528
 
  double r = -(27.0*d) + b*(9.0*c - 2.0*(b*b));
529
 
  r /= 54.0;
530
 
 
531
 
  double discrim = q*q*q + r*r;
532
 
  imag[0] = 0.0; // The first root is always real.
533
 
  double term1 = (b/3.0);
534
 
 
535
 
  if (discrim > 0) { // one root is real, two are complex
536
 
    double s = r + sqrt(discrim);
537
 
    s = ((s < 0) ? -pow(-s, (1.0/3.0)) : pow(s, (1.0/3.0)));
538
 
    double t = r - sqrt(discrim);
539
 
    t = ((t < 0) ? -pow(-t, (1.0/3.0)) : pow(t, (1.0/3.0)));
540
 
    real[0] = -term1 + s + t;
541
 
    term1 += (s + t)/2.0;
542
 
    real[1] = real[2] = -term1;
543
 
    term1 = sqrt(3.0)*(-t + s)/2;
544
 
    imag[1] = term1;
545
 
    imag[2] = -term1;
546
 
    return;
547
 
  }
548
 
 
549
 
  // The remaining options are all real
550
 
  imag[1] = imag[2] = 0.0;
551
 
  
552
 
  double r13;
553
 
  if (discrim == 0){ // All roots real, at least two are equal.
554
 
    r13 = ((r < 0) ? -pow(-r,(1.0/3.0)) : pow(r,(1.0/3.0)));
555
 
    real[0] = -term1 + 2.0*r13;
556
 
    real[1] = real[2] = -(r13 + term1);
557
 
    return;
558
 
  }
559
 
 
560
 
  // Only option left is that all roots are real and unequal (to get
561
 
  // here, q < 0)
562
 
  q = -q;
563
 
  double dum1 = q*q*q;
564
 
  dum1 = acos(r/sqrt(dum1));
565
 
  r13 = 2.0*sqrt(q);
566
 
  real[0] = -term1 + r13*cos(dum1/3.0);
567
 
  real[1] = -term1 + r13*cos((dum1 + 2.0*M_PI)/3.0);
568
 
  real[2] = -term1 + r13*cos((dum1 + 4.0*M_PI)/3.0);
569
 
}
570
 
 
571
 
void  eigsort(double d[3])
572
 
{
573
 
  int k, j, i;
574
 
  double p;
575
 
  
576
 
  for (i=0; i<3; i++) {
577
 
    p=d[k=i];
578
 
    for (j=i+1;j<3;j++)
579
 
      if (d[j] >= p) p=d[k=j];
580
 
    if (k != i) {
581
 
      d[k]=d[i];
582
 
      d[i]=p;
583
 
    }
584
 
  }
585
 
}