← Back to branch summary

~ubuntu-branches/ubuntu/utopic/gdis/utopic

« back to all changes in this revision

Viewing changes to numeric.c

  • Committer: Bazaar Package Importer
  • Author(s): Noèl Köthe
  • Date: 2004-09-13 23:44:50 UTC
  • mfrom: (1.2.1 upstream) (2.1.1 warty)
  • Revision ID: james.westby@ubuntu.com-20040913234450-wwdqtqfx9k38qhif
Tags: 0.86-1
new upstream release from 2004-08-26

Show diffs side-by-side

added added

removed removed

Lines of Context:
numeric.c
 
1
/*
 
2
Copyright (C) 2003 by Sean David Fleming
 
3
 
 
4
sean@power.curtin.edu.au
 
5
 
 
6
This program is free software; you can redistribute it and/or
 
7
modify it under the terms of the GNU General Public License
 
8
as published by the Free Software Foundation; either version 2
 
9
of the License, or (at your option) any later version.
 
10
 
 
11
This program is distributed in the hope that it will be useful,
 
12
but WITHOUT ANY WARRANTY; without even the implied warranty of
 
13
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
14
GNU General Public License for more details.
 
15
 
 
16
You should have received a copy of the GNU General Public License
 
17
along with this program; if not, write to the Free Software
 
18
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 
19
 
 
20
The GNU GPL can also be found at http://www.gnu.org
 
21
*/
 
22
 
 
23
#include <math.h>
 
24
#include <stdio.h>
 
25
#include <sys/time.h>
 
26
 
 
27
#include "gdis.h"
 
28
#include "numeric.h"
 
29
 
 
30
#define T_STEP 0.001
 
31
#define T_STEP_INV 1.0/T_STEP
 
32
 
 
33
#define ITMAX 100
 
34
#define EPS 3.0e-7
 
35
 
 
36
gdouble *s_data;
 
37
gint s_size=0;
 
38
gint sqrt_method=0;
 
39
 
 
40
/**********/
 
41
/* timing */
 
42
/**********/
 
43
gulong mytimer(void)
 
44
{
 
45
glong usec;
 
46
struct timeval tv;
 
47
 
 
48
#ifndef __WIN32
 
49
gettimeofday(&tv, NULL);
 
50
#else
 
51
/* TODO */
 
52
#endif
 
53
 
 
54
usec = tv.tv_usec + 1000000*tv.tv_sec;
 
55
 
 
56
return(usec);
 
57
}
 
58
 
 
59
/************************************/
 
60
/* initialize the trig lookup table */
 
61
/************************************/
 
62
#define DEBUG_TRIG 0
 
63
void init_trig(void)
 
64
{
 
65
gint i;
 
66
gdouble a,s;
 
67
 
 
68
/* alloc */
 
69
s_size = PI/T_STEP + 1;
 
70
s_data = (gdouble *) g_malloc(s_size * sizeof(gdouble));
 
71
 
 
72
#if DEBUG_TRIG
 
73
printf("Initializing table: %d points\n", s_size);
 
74
#endif
 
75
 
 
76
i=0;
 
77
for (a=0.0 ; a<=PI ; a+= T_STEP)
 
78
  {
 
79
  s = sin(a);
 
80
  *(s_data+i) = s;
 
81
  i++;
 
82
  }
 
83
g_assert(i == s_size);
 
84
}
 
85
 
 
86
/********************/
 
87
/* SINE replacement */
 
88
/********************/
 
89
gdouble tbl_sin(gdouble angle)
 
90
{
 
91
gint quad, idx, idx2;
 
92
gdouble s,a,s1,s2,rem;
 
93
 
 
94
/* enforce range [0, 2PI] */
 
95
ca_rad(&angle);
 
96
 
 
97
/* determine quadrant */
 
98
a = angle;
 
99
quad = 0;
 
100
while (a > PI/2.0)
 
101
  {
 
102
  a -= PI/2.0;
 
103
  quad++;
 
104
  }
 
105
/* setup angle and sign accordingly */
 
106
s = 1.0;
 
107
switch (quad)
 
108
  {
 
109
  case 1:
 
110
    a += PI/2.0;
 
111
    break;
 
112
  case 3:
 
113
    a += PI/2.0;
 
114
  case 2:
 
115
    s *= -1.0;
 
116
    break;
 
117
  }
 
118
/* init retrieval indices */
 
119
idx2 = idx = (gint) (T_STEP_INV * a);
 
120
if (idx2)
 
121
  idx2--;
 
122
else
 
123
  idx2++;
 
124
 
 
125
g_assert(idx < s_size);
 
126
g_assert(idx2 >= 0);
 
127
 
 
128
/* get the two values */
 
129
s1 = *(s_data+idx) * s;
 
130
s2 = *(s_data+idx2) * s;
 
131
/* weighted average */
 
132
rem = T_STEP_INV * a - (gint) (T_STEP_INV * a);
 
133
s = (rem*s2 + (1.0-rem)*s1);
 
134
 
 
135
return(s);
 
136
}
 
137
 
 
138
/**********************/
 
139
/* COSINE replacement */
 
140
/**********************/
 
141
gdouble tbl_cos(gdouble angle)
 
142
{
 
143
return(tbl_sin(angle + PI/2.0));
 
144
}
 
145
 
 
146
/***************************/
 
147
/* constrain angle [0,2pi] */
 
148
/***************************/
 
149
void ca_rad(gdouble *angle)
 
150
{
 
151
gint m;
 
152
 
 
153
m = *angle/(2.0*PI);
 
154
 
 
155
*angle -= (gdouble) m*2.0*PI;
 
156
 
 
157
if (*angle < 0.0)
 
158
  *angle += 2.0*PI;
 
159
}
 
160
 
 
161
/***************************/
 
162
/* constrain angle [0,360] */
 
163
/***************************/
 
164
void ca_deg(gdouble *angle)
 
165
{
 
166
gint m;
 
167
 
 
168
m = *angle/360.0;
 
169
 
 
170
*angle -= (gdouble) m*360.0;
 
171
 
 
172
if (*angle < 0.0)
 
173
  *angle += 360.0;
 
174
}
 
175
 
 
176
/********************/
 
177
/* SQRT replacement */
 
178
/********************/
 
179
/* NB: s [0.1, 1.0] */
 
180
gdouble fast_sqrt(gdouble s)
 
181
{
 
182
gdouble ds, r;
 
183
 
 
184
if (sqrt_method)
 
185
  {
 
186
  r = 0.188030699 + 1.48359853*s - 1.0979059*s*s + 0.430357353*s*s*s;
 
187
  do
 
188
    {
 
189
    ds = s - r*r;
 
190
    r += 0.5*ds/r;
 
191
    }
 
192
  while (ds > FRACTION_TOLERANCE);
 
193
  }
 
194
else
 
195
  return(sqrt(s));
 
196
 
 
197
return(r);
 
198
}
 
199
 
 
200
/**************************************************/
 
201
/* test if approx sqrt is faster than system sqrt */
 
202
/**************************************************/
 
203
void init_sqrt(void)
 
204
{
 
205
/* TODO - test if it is faster */
 
206
sqrt_method = 1;
 
207
}
 
208
 
 
209
/*******************/
 
210
/* numerical setup */
 
211
/*******************/
 
212
void init_math(void)
 
213
{
 
214
init_trig();
 
215
init_sqrt();
 
216
}
 
217
 
 
218
/********************/
 
219
/* rounding routine */
 
220
/********************/
 
221
gdouble decimal_round(gdouble x, gint dp)
 
222
{
 
223
gint i;
 
224
gdouble y, f;
 
225
 
 
226
f = pow(10.0, dp);
 
227
y = x * f;
 
228
y += 0.5;
 
229
i = y;
 
230
y = i;
 
231
y /= f;
 
232
 
 
233
return(y);
 
234
}
 
235
 
 
236
/**********/
 
237
/* gammln */
 
238
/**********/
 
239
gdouble gammln(gdouble xx)
 
240
{
 
241
gint j;
 
242
gdouble x,tmp,ser;
 
243
static gdouble cof[6]={76.18009173,-86.50532033,24.01409822,
 
244
                       -1.231739516,0.120858003e-2,-0.536382e-5};
 
245
x = xx-1.0;
 
246
tmp = x+5.5;
 
247
tmp -= (x+0.5)*log(tmp);
 
248
ser = 1.0;
 
249
for (j=0 ; j<=5 ; j++)
 
250
  {
 
251
  x += 1.0;
 
252
  ser += cof[j]/x;
 
253
  }
 
254
return -tmp+log(2.50662827465*ser);
 
255
}
 
256
 
 
257
/*******/
 
258
/* gcf */
 
259
/*******/
 
260
void gcf(gdouble *gammcf, gdouble a, gdouble x, gdouble *gln)
 
261
{
 
262
gint n;
 
263
gdouble gold=0.0,g,fac=1.0,b1=1.0;
 
264
gdouble b0=0.0,anf,ana,an,a1,a0=1.0;
 
265
 
 
266
*gln=gammln(a);
 
267
a1=x;
 
268
for (n=1 ; n<=ITMAX ; n++)
 
269
  {
 
270
  an = (gdouble) n;
 
271
  ana = an-a;
 
272
  a0 = (a1+a0*ana)*fac;
 
273
  b0 = (b1+b0*ana)*fac;
 
274
  anf = an*fac;
 
275
  a1 = x*a0+anf*a1;
 
276
  b1 = x*b0+anf*b1;
 
277
  if (a1) 
 
278
    {
 
279
    fac = 1.0/a1;
 
280
    g = b1*fac;
 
281
    if (fabs((g-gold)/g) < EPS)
 
282
      {
 
283
      *gammcf = exp(-x+a*log(x)-(*gln))*g;
 
284
      return;
 
285
      }
 
286
    gold = g;
 
287
    }
 
288
  }
 
289
g_assert_not_reached();
 
290
}
 
291
 
 
292
/********/
 
293
/* gser */
 
294
/********/
 
295
void gser(gdouble *gamser, gdouble a, gdouble x, gdouble *gln)
 
296
{
 
297
gint n;
 
298
gdouble sum,del,ap;
 
299
 
 
300
*gln = gammln(a);
 
301
if (x <= 0.0)
 
302
  {
 
303
  if (x < 0.0)
 
304
    g_assert_not_reached();
 
305
  *gamser=0.0;
 
306
  return;
 
307
  }
 
308
else
 
309
  {
 
310
  ap = a;
 
311
  del = sum=1.0/a;
 
312
  for (n=1 ; n<=ITMAX ; n++)
 
313
    {
 
314
    ap += 1.0;
 
315
    del *= x/ap;
 
316
    sum += del;
 
317
    if (fabs(del) < fabs(sum)*EPS)
 
318
      {
 
319
      *gamser = sum*exp(-x+a*log(x)-(*gln));
 
320
      return;
 
321
      }
 
322
    }
 
323
  g_assert_not_reached();
 
324
  return;
 
325
  }
 
326
}
 
327
 
 
328
/*********/
 
329
/* gammp */
 
330
/*********/
 
331
gdouble gammp(gdouble a, gdouble x)
 
332
{
 
333
gdouble gamser,gammcf,gln;
 
334
 
 
335
g_assert(x >= 0.0);
 
336
g_assert(a > 0.0);
 
337
 
 
338
if (x < (a+1.0))
 
339
  {
 
340
  gser(&gamser,a,x,&gln);
 
341
  return gamser;
 
342
  }
 
343
else
 
344
  {
 
345
  gcf(&gammcf,a,x,&gln);
 
346
  return 1.0-gammcf;
 
347
  }
 
348
}
 
349
 
 
350
/*********/
 
351
/* gammq */
 
352
/*********/
 
353
gdouble gammq(gdouble a, gdouble x)
 
354
{
 
355
gdouble gamser,gammcf,gln;
 
356
 
 
357
g_assert(x >= 0.0);
 
358
g_assert(a > 0.0);
 
359
 
 
360
if (x < (a+1.0))
 
361
  {
 
362
  gser(&gamser,a,x,&gln);
 
363
  return 1.0-gamser;
 
364
  }
 
365
else
 
366
  {
 
367
  gcf(&gammcf,a,x,&gln);
 
368
  return gammcf;
 
369
  }
 
370
}
 
371
 
 
372
/******************/
 
373
/* error function */
 
374
/******************/
 
375
gdouble erf(gdouble x)
 
376
{
 
377
gdouble val;
 
378
 
 
379
val = gammp(0.5, x*x);
 
380
 
 
381
if (x < 0.0)
 
382
  val *= -1.0;
 
383
 
 
384
return(val);
 
385
}
 
386
 
 
387
/********************************/
 
388
/* complementary error function */
 
389
/********************************/
 
390
gdouble erfc(gdouble x)
 
391
{
 
392
gdouble val;
 
393
 
 
394
if (x < 0.0)
 
395
  val = 1.0 + gammp(0.5, x*x);
 
396
else
 
397
  val = gammq(0.5, x*x);
 
398
 
 
399
return(val);
 
400
}
 
401
 
 
402
/****************/
 
403
/* general sort */
 
404
/****************/
 
405
void sort(gint size, gdouble *array)
 
406
{
 
407
gint i, swap;
 
408
gdouble tmp;
 
409
 
 
410
swap=1;
 
411
while (swap)
 
412
  {
 
413
  swap=0;
 
414
  for (i=1 ; i<size ; i++)
 
415
    {
 
416
/* TODO - direction flag for ascending or descending */
 
417
    if (array[i-1] > array[i])
 
418
      {
 
419
/* swap elements in array */
 
420
      tmp = array[i-1];
 
421
      array[i-1] = array[i];
 
422
      array[i] = tmp;
 
423
/* elements were swapped */
 
424
      swap++;
 
425
      }
 
426
    }
 
427
  }
 
428
}
 
429
 
 
430
/***************/
 
431
/* general min */
 
432
/***************/
 
433
gdouble min(gint size, gdouble *x)
 
434
{
 
435
gint i;
 
436
gdouble val;
 
437
 
 
438
g_assert(size > 0);
 
439
 
 
440
val = x[0];
 
441
 
 
442
for (i=1; i<size ; i++)
 
443
  if (x[i] < val)
 
444
    val = x[i];
 
445
 
 
446
return(val);
 
447
}
 
448
 
 
449
/***************/
 
450
/* general max */
 
451
/***************/
 
452
gdouble max(gint size, gdouble *x)
 
453
{
 
454
gint i;
 
455
gdouble val;
 
456
 
 
457
g_assert(size > 0);
 
458
 
 
459
val = x[0];
 
460
 
 
461
for (i=1; i<size ; i++)
 
462
  if (x[i] > val)
 
463
    val = x[i];
 
464
 
 
465
return(val);
 
466
}
 
467
 
 
468
/************************************/
 
469
/* numerical recipes - spline setup */
 
470
/************************************/
 
471
void spline(double *x, double *y, int n, double yp1, double ypn, double *y2)
 
472
{
 
473
int i, k;
 
474
double p, qn, sig, un, *u;
 
475
 
 
476
/* allocate 1 extra double as some people insist on starting at 1 */
 
477
u = g_malloc(n*sizeof(double));
 
478
 
 
479
if (yp1 > 0.99e30)
 
480
  y2[1] = u[1] = 0.0;
 
481
else
 
482
  {
 
483
  y2[1] = -0.5;
 
484
  u[1] = (3.0/(x[2]-x[1]))*((y[2]-y[1])/(x[2]-x[1])-yp1);
 
485
  }
 
486
 
 
487
for (i=2 ; i<=n-1 ; i++)
 
488
  {
 
489
  sig = (x[i]-x[i-1])/(x[i+1]-x[i-1]);
 
490
  p = sig*y2[i-1]+2.0;
 
491
  y2[i] = (sig-1.0)/p;
 
492
  u[i] = (y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
 
493
  u[i] = (6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
 
494
  }
 
495
 
 
496
if (ypn > 0.99e30)
 
497
  qn = un = 0.0;
 
498
else
 
499
  {
 
500
  qn = 0.5;
 
501
  un = (3.0/(x[n]-x[n-1]))*(ypn-(y[n]-y[n-1])/(x[n]-x[n-1]));
 
502
  }
 
503
 
 
504
y2[n] = (un-qn*u[n-1])/(qn*y2[n-1]+1.0);
 
505
 
 
506
for (k=n-1 ; k>=1 ; k--)
 
507
  y2[k] = y2[k]*y2[k+1]+u[k];
 
508
 
 
509
g_free(u);
 
510
}
 
511
 
 
512
/********************************************/
 
513
/* numerical recipes - spline interpolation */
 
514
/********************************************/
 
515
void splint(double *xa, double *ya, double *y2a, int n, double x, double *y)
 
516
{
 
517
int klo, khi, k;
 
518
double h, b, a;
 
519
 
 
520
klo = 1;
 
521
khi = n;
 
522
 
 
523
while (khi-klo > 1)
 
524
  {
 
525
  k = (khi+klo) >> 1;
 
526
  if (xa[k] > x)
 
527
    khi = k;
 
528
  else
 
529
    klo = k;
 
530
  }
 
531
 
 
532
h = xa[khi]-xa[klo];
 
533
if (h == 0.0)
 
534
  printf("splint(): bad xa input.\n");
 
535
 
 
536
a = (xa[khi]-x)/h;
 
537
b = (x-xa[klo])/h;
 
538
    
 
539
*y = a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h) / 6.0;
 
540
}
 
541
 
 
542
/****************************************/
 
543
/* numerical recipes - Cooley-Tukey FFT */
 
544
/****************************************/
 
545
#define SWAP(a, b) tempr=(a) ; (a) = (b) ; (b) = tempr;
 
546
void fft(gdouble *x, gint nn, gint isign)
 
547
{
 
548
gint n, mmax, m, j, istep, i;
 
549
gdouble wtemp, wr, wpr, wpi, wi, theta;
 
550
gdouble tempr, tempi;
 
551
gdouble *data = --x;    /* silly fortran programmers */
 
552
 
 
553
n = nn << 1;
 
554
j = 1;
 
555
 
 
556
for (i=1 ; i<n ; i+=2)
 
557
  {
 
558
  if (j > i)
 
559
    {
 
560
    SWAP(data[j], data[i]);
 
561
    SWAP(data[j+1], data[i+1]);
 
562
    }
 
563
  m = n >> 1;
 
564
  while (m >= 2 && j > m)
 
565
    {
 
566
    j -= m;
 
567
    m >>= 1;
 
568
    }
 
569
  j += m;
 
570
  }
 
571
 
 
572
mmax = 2;
 
573
while (n > mmax)
 
574
  {
 
575
  istep = 2 * mmax;
 
576
  theta = 2.0 * PI / (isign * mmax);
 
577
  wtemp = sin(0.5*theta);
 
578
  wpr = -2.0 * wtemp * wtemp;
 
579
  wpi = sin(theta);
 
580
  wr = 1.0;
 
581
  wi = 0.0;
 
582
  for (m=1 ; m<mmax ; m+=2 )
 
583
    {
 
584
    for (i=m ; i<=n ; i+=istep)
 
585
      {
 
586
      j = i + mmax;
 
587
      tempr =  wr * data[j] - wi * data[j+1];
 
588
      tempi =  wr * data[j+1] - wi * data[j];
 
589
      data[j] = data[i] - tempr;
 
590
      data[j+1] = data[i+1] - tempi;
 
591
      data[i] += tempr;
 
592
      data[i+1] += tempi;
 
593
      }
 
594
    wr = (wtemp = wr) * wpr - wi * wpi + wr;
 
595
    wi = wi * wpr + wtemp * wpi + wi;
 
596
    }
 
597
  mmax = istep;
 
598
  }
 
599
}
 
600