aRts audio server
Nevar pievienot vairāk kā 25 tēmas Tēmai ir jāsākas ar burtu vai ciparu, tā var saturēt domu zīmes ('-') un var būt līdz 35 simboliem gara.

gslmath.c 31KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085
  1. /* GSL - Generic Sound Layer
  2. * Copyright (C) 2001 Stefan Westerfeld and Tim Janik
  3. *
  4. * This library is free software; you can redistribute it and/or
  5. * modify it under the terms of the GNU Lesser General Public
  6. * License as published by the Free Software Foundation; either
  7. * version 2 of the License, or (at your option) any later version.
  8. *
  9. * This library is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  12. * Lesser General Public License for more details.
  13. *
  14. * You should have received a copy of the GNU Lesser General
  15. * Public License along with this library; if not, write to the
  16. * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  17. * Boston, MA 02110-1301, USA.
  18. */
  19. #include "gslmath.h"
  20. #include <string.h>
  21. #include <stdio.h>
  22. #define RING_BUFFER_LENGTH (16)
  23. #define PRINTF_DIGITS "1270"
  24. #define FLOAT_STRING_SIZE (2048)
  25. /* factorization constants: 2^(1/12), ln(2^(1/12)) and 2^(1/(12*6))
  26. * retrived with:
  27. #include <stl.h>
  28. #include <complex.h>
  29. typedef long double ld;
  30. int main (void)
  31. {
  32. ld r, l;
  33. cout.precision(256);
  34. r = pow ((ld) 2, (ld) 1 / (ld) 12);
  35. cout << "2^(1/12) =\n";
  36. cout << "2^" << (ld) 1 / (ld) 12 << " =\n";
  37. cout << r << "\n";
  38. l = log (r);
  39. cout << "ln(2^(1/12)) =\n";
  40. cout << "ln(" << r << ") =\n";
  41. cout << l << "\n";
  42. r = pow ((ld) 2, (ld) 1 / (ld) 72);
  43. cout << "2^(1/72) =\n";
  44. cout << "2^" << (ld) 1 / (ld) 72 << " =\n";
  45. cout << r << "\n";
  46. return 0;
  47. }
  48. */
  49. /* --- prototypes --- */
  50. static void zrhqr (double a[], int m, double rtr[], double rti[]);
  51. static double rf (double x, double y, double z);
  52. static double ellf (double phi, double ak);
  53. static void sncndn (double uu, double emmc, double *sn_p, double *cn_p, double *dn_p);
  54. static void sncndnC (GslComplex uu, GslComplex emmc, GslComplex *sn_p, GslComplex *cn_p, GslComplex *dn_p);
  55. static GslComplex rfC (GslComplex x, GslComplex y, GslComplex z);
  56. /* --- functions --- */
  57. static inline char*
  58. pretty_print_double (char *str,
  59. double d)
  60. {
  61. char *s= str;
  62. sprintf (s, "%."PRINTF_DIGITS"f", d);
  63. while (*s) s++;
  64. while (s[-1] == '0' && s[-2] != '.')
  65. s--;
  66. *s = 0;
  67. return s;
  68. }
  69. char*
  70. gsl_complex_list (unsigned int n_points,
  71. GslComplex *points,
  72. const char *indent)
  73. {
  74. static unsigned int rbi = 0;
  75. static char* rbuffer[RING_BUFFER_LENGTH] = { NULL, };
  76. char *s, *tbuffer = g_newa (char, (FLOAT_STRING_SIZE * 2 * n_points));
  77. unsigned int i;
  78. rbi++; if (rbi >= RING_BUFFER_LENGTH) rbi -= RING_BUFFER_LENGTH;
  79. if (rbuffer[rbi] != NULL)
  80. g_free (rbuffer[rbi]);
  81. s = tbuffer;
  82. for (i = 0; i < n_points; i++)
  83. {
  84. *s = 0;
  85. if (indent)
  86. strcat (s, indent);
  87. while (*s) s++;
  88. s = pretty_print_double (s, points[i].re);
  89. *s++ = ' ';
  90. s = pretty_print_double (s, points[i].im);
  91. *s++ = '\n';
  92. }
  93. *s++ = 0;
  94. rbuffer[rbi] = g_strdup (tbuffer);
  95. return rbuffer[rbi];
  96. }
  97. char*
  98. gsl_complex_str (GslComplex c)
  99. {
  100. static unsigned int rbi = 0;
  101. static char* rbuffer[RING_BUFFER_LENGTH] = { NULL, };
  102. char *s, tbuffer[FLOAT_STRING_SIZE * 2];
  103. rbi++; if (rbi >= RING_BUFFER_LENGTH) rbi -= RING_BUFFER_LENGTH;
  104. if (rbuffer[rbi] != NULL)
  105. g_free (rbuffer[rbi]);
  106. s = tbuffer;
  107. *s++ = '{';
  108. s = pretty_print_double (s, c.re);
  109. *s++ = ',';
  110. *s++ = ' ';
  111. s = pretty_print_double (s, c.im);
  112. *s++ = '}';
  113. *s++ = 0;
  114. rbuffer[rbi] = g_strdup (tbuffer);
  115. return rbuffer[rbi];
  116. }
  117. char*
  118. gsl_poly_str (unsigned int degree,
  119. double *a,
  120. const char *var)
  121. {
  122. static unsigned int rbi = 0;
  123. static char* rbuffer[RING_BUFFER_LENGTH] = { NULL, };
  124. char *s, *tbuffer = g_newa (char, degree * FLOAT_STRING_SIZE);
  125. unsigned int i;
  126. if (!var)
  127. var = "x";
  128. rbi++; if (rbi >= RING_BUFFER_LENGTH) rbi -= RING_BUFFER_LENGTH;
  129. if (rbuffer[rbi] != NULL)
  130. g_free (rbuffer[rbi]);
  131. s = tbuffer;
  132. *s++ = '(';
  133. s = pretty_print_double (s, a[0]);
  134. for (i = 1; i <= degree; i++)
  135. {
  136. *s++ = '+';
  137. *s = 0; strcat (s, var); while (*s) s++;
  138. *s++ = '*';
  139. *s++ = '(';
  140. s = pretty_print_double (s, a[i]);
  141. }
  142. while (i--)
  143. *s++ = ')';
  144. *s++ = 0;
  145. rbuffer[rbi] = g_strdup (tbuffer);
  146. return rbuffer[rbi];
  147. }
  148. char*
  149. gsl_poly_str1 (unsigned int degree,
  150. double *a,
  151. const char *var)
  152. {
  153. static unsigned int rbi = 0;
  154. static char* rbuffer[RING_BUFFER_LENGTH] = { NULL, };
  155. char *s, *tbuffer = g_newa (char, degree * FLOAT_STRING_SIZE);
  156. unsigned int i, need_plus = 0;
  157. if (!var)
  158. var = "x";
  159. rbi++; if (rbi >= RING_BUFFER_LENGTH) rbi -= RING_BUFFER_LENGTH;
  160. if (rbuffer[rbi] != NULL)
  161. g_free (rbuffer[rbi]);
  162. s = tbuffer;
  163. *s++ = '(';
  164. if (a[0] != 0.0)
  165. {
  166. s = pretty_print_double (s, a[0]);
  167. need_plus = 1;
  168. }
  169. for (i = 1; i <= degree; i++)
  170. {
  171. if (a[i] == 0.0)
  172. continue;
  173. if (need_plus)
  174. {
  175. *s++ = ' ';
  176. *s++ = '+';
  177. *s++ = ' ';
  178. }
  179. if (a[i] != 1.0)
  180. {
  181. s = pretty_print_double (s, a[i]);
  182. *s++ = '*';
  183. }
  184. *s = 0;
  185. strcat (s, var);
  186. while (*s) s++;
  187. if (i > 1)
  188. {
  189. *s++ = '*';
  190. *s++ = '*';
  191. sprintf (s, "%u", i);
  192. while (*s) s++;
  193. }
  194. need_plus = 1;
  195. }
  196. *s++ = ')';
  197. *s++ = 0;
  198. rbuffer[rbi] = g_strdup (tbuffer);
  199. return rbuffer[rbi];
  200. }
  201. void
  202. gsl_complex_gnuplot (const char *file_name,
  203. unsigned int n_points,
  204. GslComplex *points)
  205. {
  206. FILE *fout = fopen (file_name, "w");
  207. fputs (gsl_complex_list (n_points, points, ""), fout);
  208. fclose (fout);
  209. }
  210. double
  211. gsl_temp_freq (double kammer_freq,
  212. int halftone_delta)
  213. {
  214. double factor;
  215. factor = pow (GSL_2_POW_1_DIV_12, halftone_delta);
  216. return kammer_freq * factor;
  217. }
  218. void
  219. gsl_poly_from_re_roots (unsigned int degree,
  220. double *a,
  221. GslComplex *roots)
  222. {
  223. unsigned int i;
  224. /* initialize polynomial */
  225. a[1] = 1;
  226. a[0] = -roots[0].re;
  227. /* monomial factor multiplication */
  228. for (i = 1; i < degree; i++)
  229. {
  230. unsigned int j;
  231. a[i + 1] = a[i];
  232. for (j = i; j >= 1; j--)
  233. a[j] = a[j - 1] - a[j] * roots[i].re;
  234. a[0] *= -roots[i].re;
  235. }
  236. }
  237. void
  238. gsl_cpoly_from_roots (unsigned int degree,
  239. GslComplex *c,
  240. GslComplex *roots)
  241. {
  242. unsigned int i;
  243. /* initialize polynomial */
  244. c[1].re = 1;
  245. c[1].im = 0;
  246. c[0].re = -roots[0].re;
  247. c[0].im = -roots[0].im;
  248. /* monomial factor multiplication */
  249. for (i = 1; i < degree; i++)
  250. {
  251. GslComplex r = gsl_complex (-roots[i].re, -roots[i].im);
  252. unsigned int j;
  253. c[i + 1] = c[i];
  254. for (j = i; j >= 1; j--)
  255. c[j] = gsl_complex_add (c[j - 1], gsl_complex_mul (c[j], r));
  256. c[0] = gsl_complex_mul (c[0], r);
  257. }
  258. }
  259. void
  260. gsl_poly_complex_roots (unsigned int degree,
  261. double *a, /* [0..degree] (degree+1 elements) */
  262. GslComplex *roots) /* [degree] */
  263. {
  264. double *roots_re = g_newa (double, 1 + degree);
  265. double *roots_im = g_newa (double, 1 + degree);
  266. unsigned int i;
  267. zrhqr (a, degree, roots_re, roots_im);
  268. for (i = 0; i < degree; i++)
  269. {
  270. roots[i].re = roots_re[1 + i];
  271. roots[i].im = roots_im[1 + i];
  272. }
  273. }
  274. double
  275. gsl_ellip_rf (double x,
  276. double y,
  277. double z)
  278. {
  279. return rf (x, y, z);
  280. }
  281. double
  282. gsl_ellip_F (double phi,
  283. double ak)
  284. {
  285. return ellf (phi, ak);
  286. }
  287. double
  288. gsl_ellip_sn (double u,
  289. double emmc)
  290. {
  291. double sn;
  292. sncndn (u, emmc, &sn, NULL, NULL);
  293. return sn;
  294. }
  295. double
  296. gsl_ellip_asn (double y,
  297. double emmc)
  298. {
  299. return y * rf (1.0 - y * y, 1.0 - y * y * (1.0 - emmc), 1.0);
  300. }
  301. GslComplex
  302. gsl_complex_ellip_asn (GslComplex y,
  303. GslComplex emmc)
  304. {
  305. return gsl_complex_mul (y,
  306. rfC (gsl_complex_sub (gsl_complex (1.0, 0),
  307. gsl_complex_mul (y, y)),
  308. gsl_complex_sub (gsl_complex (1.0, 0),
  309. gsl_complex_mul3 (y, y, gsl_complex_sub (gsl_complex (1.0, 0),
  310. emmc))),
  311. gsl_complex (1.0, 0)));
  312. }
  313. GslComplex
  314. gsl_complex_ellip_sn (GslComplex u,
  315. GslComplex emmc)
  316. {
  317. GslComplex sn;
  318. sncndnC (u, emmc, &sn, NULL, NULL);
  319. return sn;
  320. }
  321. double
  322. gsl_bit_depth_epsilon (guint n_bits)
  323. {
  324. /* epsilon for various bit depths, based on significance of one bit,
  325. * minus fudge. created with:
  326. * { echo "scale=40"; for i in `seq 1 32` ; do echo "1/2^$i - 10^-($i+1)" ; done } | bc | sed 's/$/,/'
  327. */
  328. static const double bit_epsilons[] = {
  329. .4900000000000000000000000000000000000000,
  330. .2490000000000000000000000000000000000000,
  331. .1249000000000000000000000000000000000000,
  332. .0624900000000000000000000000000000000000,
  333. .0312490000000000000000000000000000000000,
  334. .0156249000000000000000000000000000000000,
  335. .0078124900000000000000000000000000000000,
  336. .0039062490000000000000000000000000000000,
  337. .0019531249000000000000000000000000000000,
  338. .0009765624900000000000000000000000000000,
  339. .0004882812490000000000000000000000000000,
  340. .0002441406249000000000000000000000000000,
  341. .0001220703124900000000000000000000000000,
  342. .0000610351562490000000000000000000000000,
  343. .0000305175781249000000000000000000000000,
  344. .0000152587890624900000000000000000000000,
  345. .0000076293945312490000000000000000000000,
  346. .0000038146972656249000000000000000000000,
  347. .0000019073486328124900000000000000000000,
  348. .0000009536743164062490000000000000000000,
  349. .0000004768371582031249000000000000000000,
  350. .0000002384185791015624900000000000000000,
  351. .0000001192092895507812490000000000000000,
  352. .0000000596046447753906249000000000000000,
  353. .0000000298023223876953124900000000000000,
  354. .0000000149011611938476562490000000000000,
  355. .0000000074505805969238281249000000000000,
  356. .0000000037252902984619140624900000000000,
  357. .0000000018626451492309570312490000000000,
  358. .0000000009313225746154785156249000000000,
  359. .0000000004656612873077392578124900000000,
  360. .0000000002328306436538696289062490000000,
  361. };
  362. return bit_epsilons[CLAMP (n_bits, 1, 32) - 1];
  363. }
  364. /* --- Numerical Receipes --- */
  365. #define gsl_complex_rmul(scale, c) gsl_complex_scale (c, scale)
  366. #define ONE gsl_complex (1.0, 0)
  367. #define SIGN(a,b) ((b) >= 0.0 ? fabs (a) : -fabs(a))
  368. static inline int IMAX (int i1, int i2) { return i1 > i2 ? i1 : i2; }
  369. static inline double DMIN (double d1, double d2) { return d1 < d2 ? d1 : d2; }
  370. static inline double DMAX (double d1, double d2) { return d1 > d2 ? d1 : d2; }
  371. static inline double DSQR (double d) { return d == 0.0 ? 0.0 : d * d; }
  372. #define nrerror(error) g_error ("NR-ERROR: %s", (error))
  373. static inline double* vector (long nl, long nh)
  374. /* allocate a vector with subscript range v[nl..nh] */
  375. {
  376. double *v = g_new (double, nh - nl + 1 + 1);
  377. return v - nl + 1;
  378. }
  379. static inline void free_vector (double *v, long nl, long nh)
  380. {
  381. g_free (v + nl - 1);
  382. }
  383. static inline double** matrix (long nrl, long nrh, long ncl, long nch)
  384. /* allocate a matrix with subscript range m[nrl..nrh][ncl..nch] */
  385. {
  386. long i, nrow = nrh - nrl + 1, ncol = nch - ncl + 1;
  387. double **m = g_new (double*, nrow + 1);
  388. m += 1;
  389. m -= nrl;
  390. m[nrl] = g_new (double, nrow * ncol + 1);
  391. m[nrl] += 1;
  392. m[nrl] -= ncl;
  393. for (i = nrl + 1; i <= nrh; i++)
  394. m[i] = m[i - 1] + ncol;
  395. return m;
  396. }
  397. static inline void free_matrix (double **m, long nrl, long nrh, long ncl, long nch)
  398. {
  399. g_free (m[nrl] + ncl - 1);
  400. g_free (m + nrl - 1);
  401. }
  402. static void
  403. poldiv (double u[], int n, double v[], int nv, double q[], double r[])
  404. /* Given the n+1 coefficients of a polynomial of degree n in u[0..n], and the nv+1 coefficients
  405. of another polynomial of degree nv in v[0..nv], divide the polynomial u by the polynomial
  406. v ("u"/"v") giving a quotient polynomial whose coefficients are returned in q[0..n], and a
  407. remainder polynomial whose coefficients are returned in r[0..n]. The elements r[nv..n]
  408. and q[n-nv+1..n] are returned as zero. */
  409. {
  410. int k,j;
  411. for (j=0;j<=n;j++) {
  412. r[j]=u[j];
  413. q[j]=0.0;
  414. }for (k=n-nv;k>=0;k--) {
  415. q[k]=r[nv+k]/v[nv];
  416. for (j=nv+k-1;j>=k;j--) r[j] -= q[k]*v[j-k];
  417. }for (j=nv;j<=n;j++) r[j]=0.0;
  418. }
  419. #define MAX_ITER_BASE 9 /* TIMJ: was 3 */
  420. #define MAX_ITER_FAC 20 /* TIMJ: was 10 */
  421. static void
  422. hqr (double **a, int n, double wr[], double wi[])
  423. /* Finds all eigenvalues of an upper Hessenberg matrix a[1..n][1..n]. On input a can be
  424. exactly as output from elmhes §11.5; on output it is destroyed. The real and imaginary parts
  425. of the eigenvalues are returned in wr[1..n] and wi[1..n], respectively. */
  426. {
  427. int nn,m,l,k,j,its,i,mmin;
  428. double z,y,x,w,v,u,t,s,r,q,p,anorm;
  429. r=q=p=0; /* TIMJ: silence compiler */
  430. anorm=0.0; /* Compute matrix norm for possible use in lo- */
  431. for (i=1;i<=n;i++) /* cating single small subdiagonal element. */
  432. for (j=IMAX (i-1,1);j<=n;j++)
  433. anorm += fabs (a[i][j]);
  434. nn=n;
  435. t=0.0; /* Gets changed only by an exceptional shift. */
  436. while (nn >= 1) { /* Begin search for next eigenvalue. */
  437. its=0;
  438. do {for (l=nn;l>=2;l--) { /* Begin iteration: look for single small subdi- */
  439. s=fabs (a[l-1][l-1])+fabs (a[l][l]); /* agonal element. */
  440. if (s == 0.0) s=anorm;
  441. if ((double)(fabs (a[l][l-1]) + s) == s) break;
  442. }
  443. x=a[nn][nn];
  444. if (l == nn) { /* One root found. */
  445. wr[nn]=x+t;
  446. wi[nn--]=0.0;
  447. } else {
  448. y=a[nn-1][nn-1];
  449. w=a[nn][nn-1]*a[nn-1][nn];
  450. if (l == (nn-1)) { /* Two roots found... */
  451. p=0.5*(y-x);
  452. q=p*p+w;
  453. z=sqrt (fabs (q));
  454. x += t;
  455. if (q >= 0.0) { /* ...a real pair. */
  456. z=p+SIGN (z,p);
  457. wr[nn-1]=wr[nn]=x+z;
  458. if (z) wr[nn]=x-w/z;
  459. wi[nn-1]=wi[nn]=0.0;
  460. } else { /* ...a complex pair. */
  461. wr[nn-1]=wr[nn]=x+p;
  462. wi[nn-1]= -(wi[nn]=z);
  463. }
  464. nn -= 2;
  465. } else { /* No roots found. Continue iteration. */
  466. if (its == MAX_ITER_BASE * MAX_ITER_FAC)
  467. nrerror ("Too many iterations in hqr");
  468. if (its && !(its%MAX_ITER_FAC)) { /* Form exceptional shift. */
  469. t += x;
  470. for (i=1;i<=nn;i++) a[i][i] -= x;
  471. s=fabs (a[nn][nn-1])+fabs (a[nn-1][nn-2]);
  472. y=x=0.75*s;
  473. w = -0.4375*s*s;
  474. }
  475. ++its;
  476. for (m=(nn-2);m>=l;m--) { /* Form shift and then look for */
  477. z=a[m][m]; /* 2 consecutive small sub- */
  478. r=x-z; /* diagonal elements. */
  479. s=y-z;
  480. p=(r*s-w)/a[m+1][m]+a[m][m+1]; /* Equation (11.6.23). */
  481. q=a[m+1][m+1]-z-r-s;
  482. r=a[m+2][m+1];
  483. s=fabs (p)+fabs (q)+fabs (r); /* Scale to prevent overflow or */
  484. p /= s; /* underflow. */
  485. q /= s;
  486. r /= s;
  487. if (m == l) break;
  488. u=fabs (a[m][m-1])*(fabs (q)+fabs (r));
  489. v=fabs (p)*(fabs (a[m-1][m-1])+fabs (z)+fabs (a[m+1][m+1]));
  490. if ((double)(u+v) == v)
  491. break; /* Equation (11.6.26). */
  492. }
  493. for (i=m+2;i<=nn;i++) {
  494. a[i][i-2]=0.0;
  495. if (i != (m+2))
  496. a[i][i-3]=0.0;
  497. }
  498. for (k=m;k<=nn-1;k++) {
  499. /* Double QR step on rows l to nn and columns m to nn. */
  500. if (k != m) {
  501. p=a[k][k-1]; /* Begin setup of Householder */
  502. q=a[k+1][k-1]; /* vector. */
  503. r=0.0;
  504. if (k != (nn-1)) r=a[k+2][k-1];
  505. if ((x=fabs (p)+fabs (q)+fabs (r)) != 0.0) {
  506. p /= x; /* Scale to prevent overflow or */
  507. q /= x; /* underflow. */
  508. r /= x;
  509. }
  510. }
  511. if ((s=SIGN (sqrt (p*p+q*q+r*r),p)) != 0.0) {
  512. if (k == m) {
  513. if (l != m)
  514. a[k][k-1] = -a[k][k-1];
  515. } else
  516. a[k][k-1] = -s*x;
  517. p += s; /* Equations (11.6.24). */
  518. x=p/s;
  519. y=q/s;
  520. z=r/s;
  521. q /= p;
  522. r /= p;
  523. for (j=k;j<=nn;j++) { /* Row modification. */
  524. p=a[k][j]+q*a[k+1][j];
  525. if (k != (nn-1)) {
  526. p += r*a[k+2][j];
  527. a[k+2][j] -= p*z;
  528. }
  529. a[k+1][j] -= p*y;
  530. a[k][j] -= p*x;
  531. }
  532. mmin = nn<k+3 ? nn : k+3;
  533. for (i=l;i<=mmin;i++) { /* Column modification. */
  534. p=x*a[i][k]+y*a[i][k+1];
  535. if (k != (nn-1)) {
  536. p += z*a[i][k+2];
  537. a[i][k+2] -= p*r;
  538. }a[i][k+1] -= p*q;
  539. a[i][k] -= p;
  540. }
  541. }
  542. }
  543. }
  544. }
  545. } while (l < nn-1);
  546. }
  547. }
  548. #define RADIX 2.0
  549. static void
  550. balanc (double **a, int n)
  551. /* Given a matrix a[1..n][1..n], this routine replaces it by a balanced matrix with identical
  552. eigenvalues. A symmetric matrix is already balanced and is unaffected by this procedure. The
  553. parameter RADIX should be the machine's floating-point radix. */
  554. {
  555. int last,j,i;
  556. double s,r,g,f,c,sqrdx;
  557. sqrdx=RADIX*RADIX;
  558. last=0;
  559. while (last == 0) {
  560. last=1;
  561. for (i=1;i<=n;i++) { /* Calculate row and column norms. */
  562. r=c=0.0;
  563. for (j=1;j<=n;j++)
  564. if (j != i) {
  565. c += fabs (a[j][i]);
  566. r += fabs (a[i][j]);
  567. }
  568. if (c && r) { /* If both are nonzero, */
  569. g=r/RADIX;
  570. f=1.0;
  571. s=c+r;
  572. while (c<g) { /* find the integer power of the machine radix that */
  573. f *= RADIX; /* comes closest to balancing the matrix. */
  574. c *= sqrdx;
  575. }
  576. g=r*RADIX;
  577. while (c>g) {
  578. f /= RADIX;
  579. c /= sqrdx;
  580. }
  581. if ((c+r)/f < 0.95*s) {
  582. last=0;
  583. g=1.0/f;
  584. for (j=1;j<=n;j++)
  585. a[i][j] *= g; /* Apply similarity transformation */
  586. for (j=1;j<=n;j++) a[j][i] *= f;
  587. }
  588. }
  589. }
  590. }
  591. }
  592. #define MAX_DEGREE 50
  593. static void
  594. zrhqr (double a[], int m, double rtr[], double rti[])
  595. /* Find all the roots of a polynomial with real coefficients, E(i=0..m) a(i)x^i, given the degree m
  596. and the coefficients a[0..m]. The method is to construct an upper Hessenberg matrix whose
  597. eigenvalues are the desired roots, and then use the routines balanc and hqr. The real and
  598. imaginary parts of the roots are returned in rtr[1..m] and rti[1..m], respectively. */
  599. {
  600. int j,k;
  601. double **hess,xr,xi;
  602. hess=matrix (1,MAX_DEGREE,1,MAX_DEGREE);
  603. if (m > MAX_DEGREE || a[m] == 0.0 || /* TIMJ: */ fabs (a[m]) < 1e-15 )
  604. nrerror ("bad args in zrhqr");
  605. for (k=1;k<=m;k++) /* Construct the matrix. */
  606. {
  607. hess[1][k] = -a[m-k]/a[m];
  608. for (j=2;j<=m;j++)
  609. hess[j][k]=0.0;
  610. if (k != m)
  611. hess[k+1][k]=1.0;
  612. }
  613. balanc (hess,m); /* Find its eigenvalues. */
  614. hqr (hess,m,rtr,rti);
  615. if (0) /* TIMJ: don't need sorting */
  616. for (j=2;j<=m;j++)
  617. { /* Sort roots by their real parts by straight insertion. */
  618. xr=rtr[j];
  619. xi=rti[j];
  620. for (k=j-1;k>=1;k--)
  621. {
  622. if (rtr[k] <= xr)
  623. break;
  624. rtr[k+1]=rtr[k];
  625. rti[k+1]=rti[k];
  626. }
  627. rtr[k+1]=xr;
  628. rti[k+1]=xi;
  629. }
  630. free_matrix (hess,1,MAX_DEGREE,1,MAX_DEGREE);
  631. }
  632. #define EPSS 2.0e-16 /* TIMJ, was(float): 1.0e-7 */
  633. #define MR 8
  634. #define MT 100 /* TIMJ: was: 10 */
  635. #define MAXIT (MT*MR)
  636. /* Here EPSS is the estimated fractional roundoff error. We try to break (rare) limit cycles with
  637. MR different fractional values, once every MT steps, for MAXIT total allowed iterations. */
  638. static void
  639. laguer (GslComplex a[], int m, GslComplex *x, int *its)
  640. /* Given the degree m and the m+1 complex coefficients a[0..m] of the polynomial mi=0 a[i]xi,
  641. and given a complex value x, this routine improves x by Laguerre's method until it converges,
  642. within the achievable roundoff limit, to a root of the given polynomial. The number of iterations
  643. taken is returned as its. */
  644. {
  645. int iter,j;
  646. double abx,abp,abm,err;
  647. GslComplex dx,x1,b,d,f,g,h,sq,gp,gm,g2;
  648. static double frac[MR+1] = {0.0,0.5,0.25,0.75,0.13,0.38,0.62,0.88,1.0};
  649. /* Fractions used to break a limit cycle. */
  650. for (iter=1;iter<=MAXIT;iter++)
  651. { /* Loop over iterations up to allowed maximum. */
  652. *its=iter;
  653. b=a[m];
  654. err=gsl_complex_abs (b);
  655. d=f=gsl_complex (0.0,0.0);
  656. abx=gsl_complex_abs (*x);
  657. for (j=m-1;j>=0;j--)
  658. { /* Efficient computation of the polynomial and */
  659. f=gsl_complex_add (gsl_complex_mul (*x,f),d); /* its first two derivatives. */
  660. d=gsl_complex_add (gsl_complex_mul (*x,d),b);
  661. b=gsl_complex_add (gsl_complex_mul (*x,b),a[j]);
  662. err=gsl_complex_abs (b)+abx*err;
  663. }
  664. err *= EPSS;
  665. /* Estimate of roundoff error in evaluating polynomial. */
  666. if (gsl_complex_abs (b) <= err)
  667. return; /* We are on the root. */
  668. g=gsl_complex_div (d,b); /* The generic case: use Laguerre's formula. */
  669. g2=gsl_complex_mul (g,g);
  670. h=gsl_complex_sub (g2,gsl_complex_rmul (2.0,gsl_complex_div (f,b)));
  671. sq=gsl_complex_sqrt (gsl_complex_rmul ((double) (m-1),gsl_complex_sub (gsl_complex_rmul ((double) m,h),g2)));
  672. gp=gsl_complex_add (g,sq);
  673. gm=gsl_complex_sub (g,sq);
  674. abp=gsl_complex_abs (gp);
  675. abm=gsl_complex_abs (gm);
  676. if (abp < abm)
  677. gp=gm;
  678. dx=((DMAX (abp,abm) > 0.0 ? gsl_complex_div (gsl_complex ((double) m,0.0),gp)
  679. : gsl_complex_rmul (1+abx,gsl_complex (cos ((double)iter),sin ((double)iter)))));
  680. x1=gsl_complex_sub (*x,dx);
  681. if (x->re == x1.re && x->im == x1.im)
  682. return; /* Converged. */
  683. if (iter % MT) *x=x1;
  684. else *x=gsl_complex_sub (*x,gsl_complex_rmul (frac[iter/MT],dx));
  685. /* Every so often we take a fractional step, to break any limit cycle (itself a rare occurrence). */
  686. }
  687. nrerror ("too many iterations in laguer");
  688. /* Very unusual - can occur only for complex roots. Try a different starting guess for the root. */
  689. }
  690. /* Here is a driver routine that calls laguer in succession for each root, performs
  691. the deflation, optionally polishes the roots by the same Laguerre method - if you
  692. are not going to polish in some other way - and finally sorts the roots by their real
  693. parts. (We will use this routine in Chapter 13.) */
  694. #define EPS 4.0e-15 /* TIMJ, was(float): 2.0e-6 */
  695. #define MAXM 100
  696. /* A small number, and maximum anticipated value of m. */
  697. static void
  698. zroots (GslComplex a[], int m, GslComplex roots[], int polish)
  699. /* Given the degree m and the m+1 complex coefficients a[0..m] of the polynomial mi=0 a (i)xi,
  700. this routine successively calls laguer and finds all m complex roots in roots[1..m]. The
  701. boolean variable polish should be input as true (1) if polishing (also by Laguerre's method)
  702. is desired, false (0) if the roots will be subsequently polished by other means. */
  703. {
  704. int i,its,j,jj;
  705. GslComplex x,b,c,ad[MAXM];
  706. for (j=0;j<=m;j++) ad[j]=a[j]; /* Copy of coefficients for successive deflation. */
  707. for (j=m;j>=1;j--) /* Loop over each root to be found. */
  708. {
  709. x=gsl_complex (0.0,0.0); /* Start at zero to favor convergence to small- */
  710. laguer (ad,j,&x,&its); /* est remaining root, and find the root. */
  711. if (fabs (x.im) <= 2.0*EPS*fabs (x.re))
  712. x.im=0.0;
  713. roots[j]=x;
  714. b=ad[j]; /* Forward deflation. */
  715. for (jj=j-1;jj>=0;jj--)
  716. {
  717. c=ad[jj];
  718. ad[jj]=b;
  719. b=gsl_complex_add (gsl_complex_mul (x,b),c);
  720. }
  721. }
  722. if (polish)
  723. for (j=1;j<=m;j++) /* Polish the roots using the undeflated coeffi- */
  724. laguer (a,m,&roots[j],&its); /* cients. */
  725. for (j=2;j<=m;j++) /* Sort roots by their real parts by straight insertion */
  726. {
  727. x=roots[j];
  728. for (i=j-1;i>=1;i--) {
  729. if (roots[i].re <= x.re)
  730. break;
  731. roots[i+1]=roots[i];
  732. }
  733. roots[i+1]=x;
  734. }
  735. }
  736. #define ITMAX 20 /* At most ITMAX iterations. */
  737. #define TINY 2.0-15 /* TIMJ, was (float): 1.0e-6 */
  738. static void
  739. qroot (double p[], int n, double *b, double *c, double eps)
  740. /* Given n+1 coefficients p[0..n] of a polynomial of degree n, and trial values for the coefficients
  741. of a quadratic factor x*x+b*x+c, improve the solution until the coefficients b,c change by less
  742. than eps. The routine poldiv §5.3 is used. */
  743. {
  744. int iter;
  745. double sc,sb,s,rc,rb,r,dv,delc,delb;
  746. double *q,*qq,*rem;
  747. double d[3];
  748. q=vector (0,n);
  749. qq=vector (0,n);
  750. rem=vector (0,n);
  751. d[2]=1.0;
  752. for (iter=1;iter<=ITMAX;iter++)
  753. {
  754. d[1]=(*b);
  755. d[0]=(*c);
  756. poldiv (p,n,d,2,q,rem);
  757. s=rem[0]; /* First division r,s. */
  758. r=rem[1];
  759. poldiv (q,(n-1),d,2,qq,rem);
  760. sb = -(*c)*(rc = -rem[1]); /* Second division partial r,s with respect to */
  761. rb = -(*b)*rc+(sc = -rem[0]); /* c. */
  762. dv=1.0/(sb*rc-sc*rb); /* Solve 2x2 equation. */
  763. delb=(r*sc-s*rc)*dv;
  764. delc=(-r*sb+s*rb)*dv;
  765. *b += (delb=(r*sc-s*rc)*dv);
  766. *c += (delc=(-r*sb+s*rb)*dv);
  767. if ((fabs (delb) <= eps*fabs (*b) || fabs (*b) < TINY)
  768. && (fabs (delc) <= eps*fabs (*c) || fabs (*c) < TINY))
  769. {
  770. free_vector (rem,0,n); /* Coefficients converged. */
  771. free_vector (qq,0,n);
  772. free_vector (q,0,n);
  773. return;
  774. }
  775. }
  776. nrerror ("Too many iterations in routine qroot");
  777. }
  778. #define SNCNDN_CA 0.0003 /* The accuracy is the square of SNCNDN_CA. */
  779. static void
  780. sncndn (double uu, double emmc, double *sn_p, double *cn_p, double *dn_p)
  781. /* Returns the Jacobian elliptic functions sn(u, kc), cn(u, kc), and dn(u, kc). Here uu = u, while
  782. emmc = k2c. */
  783. {
  784. double a,b,c,d,emc,u,sn,cn,dn;
  785. double em[14],en[14];
  786. int i,ii,l,bo;
  787. d=0; /* TIMJ: shutup compiler */
  788. emc=emmc;
  789. u=uu;
  790. if (emc) {
  791. bo=(emc < 0.0);
  792. if (bo) {
  793. d=1.0-emc;
  794. emc /= -1.0/d;
  795. u *= (d=sqrt(d));
  796. }a=1.0;
  797. dn=1.0;
  798. for (i=1;i<=13;i++) {
  799. l=i;
  800. em[i]=a;
  801. en[i]=(emc=sqrt(emc));
  802. c=0.5*(a+emc);
  803. if (fabs(a-emc) <= SNCNDN_CA*a) break;
  804. emc *= a;
  805. a=c;
  806. }u *= c;
  807. sn=sin(u);
  808. cn=cos(u);
  809. if (sn) {
  810. a=cn/sn;
  811. c *= a;
  812. for (ii=l;ii>=1;ii--) {
  813. b=em[ii];
  814. a *= c;
  815. c *= dn;
  816. dn=(en[ii]+a)/(b+a);
  817. a=c/b;
  818. }a=1.0/sqrt(c*c+1.0);
  819. sn=(sn >= 0.0 ? a : -a);
  820. cn=c*sn;
  821. }if (bo) {
  822. a=dn;
  823. dn=cn;
  824. cn=a;
  825. sn /= d;
  826. }
  827. } else {
  828. cn=1.0/cosh(u);
  829. dn=cn;
  830. sn=tanh(u);
  831. }
  832. if (sn_p)
  833. *sn_p = sn;
  834. if (cn_p)
  835. *cn_p = cn;
  836. if (dn_p)
  837. *dn_p = dn;
  838. }
  839. static void
  840. sncndnC (GslComplex uu, GslComplex emmc, GslComplex *sn_p, GslComplex *cn_p, GslComplex *dn_p)
  841. {
  842. GslComplex a,b,c,d,emc,u,sn,cn,dn;
  843. GslComplex em[14],en[14];
  844. int i,ii,l,bo;
  845. emc=emmc;
  846. u=uu;
  847. if (emc.re || emc.im) /* gsl_complex_abs (emc)) */
  848. {
  849. /* bo=gsl_complex_abs (emc) < 0.0; */
  850. bo=emc.re < 0.0;
  851. if (bo) {
  852. d=gsl_complex_sub (ONE, emc);
  853. emc = gsl_complex_div (emc, gsl_complex_div (gsl_complex (-1.0, 0), d));
  854. d = gsl_complex_sqrt (d);
  855. u = gsl_complex_mul (u, d);
  856. }
  857. a=ONE; dn=ONE;
  858. for (i=1;i<=13;i++) {
  859. l=i;
  860. em[i]=a;
  861. emc = gsl_complex_sqrt (emc);
  862. en[i]=emc;
  863. c = gsl_complex_mul (gsl_complex (0.5, 0), gsl_complex_add (a, emc));
  864. if (gsl_complex_abs (gsl_complex_sub (a, emc)) <=
  865. gsl_complex_abs (gsl_complex_mul (gsl_complex (SNCNDN_CA, 0), a)))
  866. break;
  867. emc = gsl_complex_mul (emc, a);
  868. a=c;
  869. }
  870. u = gsl_complex_mul (u, c);
  871. sn = gsl_complex_sin (u);
  872. cn = gsl_complex_cos (u);
  873. if (sn.re) /* gsl_complex_abs (sn)) */
  874. {
  875. a= gsl_complex_div (cn, sn);
  876. c = gsl_complex_mul (c, a);
  877. for (ii=l;ii>=1;ii--) {
  878. b = em[ii];
  879. a = gsl_complex_mul (a, c);
  880. c = gsl_complex_mul (c, dn);
  881. dn = gsl_complex_div (gsl_complex_add (en[ii], a), gsl_complex_add (b, a));
  882. a = gsl_complex_div (c, b);
  883. }
  884. a = gsl_complex_div (ONE, gsl_complex_sqrt (gsl_complex_add (ONE, gsl_complex_mul (c, c))));
  885. if (sn.re >= 0.0) /* gsl_complex_arg (sn) >= 0.0) */
  886. sn = a;
  887. else
  888. {
  889. sn.re = -a.re;
  890. sn.im = a.im;
  891. }
  892. cn = gsl_complex_mul (c, sn);
  893. }
  894. if (bo) {
  895. a=dn;
  896. dn=cn;
  897. cn=a;
  898. sn = gsl_complex_div (sn, d);
  899. }
  900. } else {
  901. cn=gsl_complex_div (ONE, gsl_complex_cosh (u));
  902. dn=cn;
  903. sn=gsl_complex_tanh (u);
  904. }
  905. if (sn_p)
  906. *sn_p = sn;
  907. if (cn_p)
  908. *cn_p = cn;
  909. if (dn_p)
  910. *dn_p = dn;
  911. }
  912. #define RF_ERRTOL 0.0025 /* TIMJ, was(float): 0.08 */
  913. #define RF_TINY 2.2e-307 /* TIMJ, was(float): 1.5e-38 */
  914. #define RF_BIG 1.5e+307 /* TIMJ, was(float): 3.0e37 */
  915. #define RF_THIRD (1.0/3.0)
  916. #define RF_C1 (1.0/24.0)
  917. #define RF_C2 0.1
  918. #define RF_C3 (3.0/44.0)
  919. #define RF_C4 (1.0/14.0)
  920. static double
  921. rf (double x, double y, double z)
  922. /* Computes Carlson's elliptic integral of the first kind, RF (x, y, z). x, y, and z must be nonneg-
  923. ative, and at most one can be zero. RF_TINY must be at least 5 times the machine underflow limit,
  924. RF_BIG at most one fifth the machine overflow limit. */
  925. {
  926. double alamb,ave,delx,dely,delz,e2,e3,sqrtx,sqrty,sqrtz,xt,yt,zt;
  927. if (1 /* TIMJ: add verbose checks */)
  928. {
  929. if (DMIN (DMIN (x, y), z) < 0.0)
  930. nrerror ("rf: x,y,z have to be positive");
  931. if (DMIN (DMIN (x + y, x + z), y + z) < RF_TINY)
  932. nrerror ("rf: only one of x,y,z may be 0");
  933. if (DMAX (DMAX (x, y), z) > RF_BIG)
  934. nrerror ("rf: at least one of x,y,z is too big");
  935. }
  936. if (DMIN(DMIN(x,y),z) < 0.0 || DMIN(DMIN(x+y,x+z),y+z) < RF_TINY ||
  937. DMAX(DMAX(x,y),z) > RF_BIG)
  938. nrerror("invalid arguments in rf");
  939. xt=x;
  940. yt=y;
  941. zt=z;
  942. do {
  943. sqrtx=sqrt(xt);
  944. sqrty=sqrt(yt);
  945. sqrtz=sqrt(zt);
  946. alamb=sqrtx*(sqrty+sqrtz)+sqrty*sqrtz;
  947. xt=0.25*(xt+alamb);
  948. yt=0.25*(yt+alamb);
  949. zt=0.25*(zt+alamb);
  950. ave=RF_THIRD*(xt+yt+zt);
  951. delx=(ave-xt)/ave;
  952. dely=(ave-yt)/ave;
  953. delz=(ave-zt)/ave;
  954. } while (DMAX(DMAX(fabs(delx),fabs(dely)),fabs(delz)) > RF_ERRTOL);
  955. e2=delx*dely-delz*delz;
  956. e3=delx*dely*delz;
  957. return (1.0+(RF_C1*e2-RF_C2-RF_C3*e3)*e2+RF_C4*e3)/sqrt(ave);
  958. }
  959. static GslComplex
  960. rfC (GslComplex x, GslComplex y, GslComplex z)
  961. {
  962. GslComplex alamb,ave,delx,dely,delz,e2,e3,sqrtx,sqrty,sqrtz,xt,yt,zt;
  963. GslComplex RFC_C1 = {1.0/24.0, 0}, RFC_C2 = {0.1, 0}, RFC_C3 = {3.0/44.0, 0}, RFC_C4 = {1.0/14.0, 0};
  964. if (DMIN (DMIN (gsl_complex_abs (x), gsl_complex_abs (y)), gsl_complex_abs (z)) < 0.0)
  965. nrerror ("rf: x,y,z have to be positive");
  966. if (DMIN (DMIN (gsl_complex_abs (x) + gsl_complex_abs (y), gsl_complex_abs (x) + gsl_complex_abs (z)),
  967. gsl_complex_abs (y) + gsl_complex_abs (z)) < RF_TINY)
  968. nrerror ("rf: only one of x,y,z may be 0");
  969. if (DMAX (DMAX (gsl_complex_abs (x), gsl_complex_abs (y)), gsl_complex_abs (z)) > RF_BIG)
  970. nrerror ("rf: at least one of x,y,z is too big");
  971. xt=x;
  972. yt=y;
  973. zt=z;
  974. do {
  975. sqrtx = gsl_complex_sqrt (xt);
  976. sqrty = gsl_complex_sqrt (yt);
  977. sqrtz = gsl_complex_sqrt (zt);
  978. alamb = gsl_complex_add (gsl_complex_mul (sqrtx, gsl_complex_add (sqrty, sqrtz)), gsl_complex_mul (sqrty, sqrtz));
  979. xt = gsl_complex_mul (gsl_complex (0.25, 0), gsl_complex_add (xt, alamb));
  980. yt = gsl_complex_mul (gsl_complex (0.25, 0), gsl_complex_add (yt, alamb));
  981. zt = gsl_complex_mul (gsl_complex (0.25, 0), gsl_complex_add (zt, alamb));
  982. ave = gsl_complex_mul (gsl_complex (RF_THIRD, 0), gsl_complex_add3 (xt, yt, zt));
  983. delx = gsl_complex_div (gsl_complex_sub (ave, xt), ave);
  984. dely = gsl_complex_div (gsl_complex_sub (ave, yt), ave);
  985. delz = gsl_complex_div (gsl_complex_sub (ave, zt), ave);
  986. /* } while (DMAX (DMAX (fabs (delx.re), fabs (dely.re)), fabs (delz.re)) > RF_ERRTOL); */
  987. } while (DMAX (DMAX (gsl_complex_abs (delx), gsl_complex_abs (dely)), gsl_complex_abs (delz)) > RF_ERRTOL);
  988. e2 = gsl_complex_sub (gsl_complex_mul (delx, dely), gsl_complex_mul (delz, delz));
  989. e3 = gsl_complex_mul3 (delx, dely, delz);
  990. return gsl_complex_div (gsl_complex_add3 (gsl_complex (1.0, 0),
  991. gsl_complex_mul (e2,
  992. gsl_complex_sub3 (gsl_complex_mul (RFC_C1, e2),
  993. RFC_C2,
  994. gsl_complex_mul (RFC_C3, e3))),
  995. gsl_complex_mul (RFC_C4, e3)),
  996. gsl_complex_sqrt (ave));
  997. }
  998. static double
  999. ellf (double phi, double ak)
  1000. /* Legendre elliptic integral of the 1st kind F(phi, k), evaluated using Carlson's function RF.
  1001. The argument ranges are 0 <= phi <= pi/2, 0 <= k*sin(phi) <= 1. */
  1002. {
  1003. double s=sin(phi);
  1004. return s*rf(DSQR(cos(phi)),(1.0-s*ak)*(1.0+s*ak),1.0);
  1005. }