Context Navigation

mbmath.c

Visit:

Last change on this file was 10416, checked in by aboeckma, 12 years ago
Added mr bayes (no menu yet)
File size: 118.3 KB

Line
1	/*
2	* MrBayes 3
3	*
4	* (c) 2002-2010
5	*
6	* John P. Huelsenbeck
7	* Dept. Integrative Biology
8	* University of California, Berkeley
9	* Berkeley, CA 94720-3140
10	* johnh@berkeley.edu
11	*
12	* Fredrik Ronquist
13	* Swedish Museum of Natural History
14	* Box 50007
15	* SE-10405 Stockholm, SWEDEN
16	* fredrik.ronquist@nrm.se
17	*
18	* With important contributions by
19	*
20	* Paul van der Mark (paulvdm@sc.fsu.edu)
21	* Maxim Teslenko (maxim.teslenko@nrm.se)
22	*
23	* and by many users (run 'acknowledgements' to see more info)
24	*
25	* This program is free software; you can redistribute it and/or
26	* modify it under the terms of the GNU General Public License
27	* as published by the Free Software Foundation; either version 2
28	* of the License, or (at your option) any later version.
29	*
30	* This program is distributed in the hope that it will be useful,
31	* but WITHOUT ANY WARRANTY; without even the implied warranty of
32	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
33	* GNU General Public License for more details (www.gnu.org).
34	*
35	*/
36
37	#include <stdio.h>
38	#include <stdlib.h>
39	#include <time.h>
40	#include <math.h>
41	#include <float.h>
42	#include <string.h>
43	#include <stdarg.h>
44	#include "mb.h"
45	#include "globals.h"
46	#include "mbmath.h"
47	#include "bayes.h"
48	#include "model.h"
49	#include "utils.h"
50
51	const char* const svnRevisionMbmathC="$Rev: 445 $"; /* Revision keyword which is expended/updated by svn on each commit/update*/
52
53	#define MAX_GAMMA_CATS 20
54	#define PI 3.14159265358979324
55	#define PIOVER2 1.57079632679489662
56	#define POINTGAMMA(prob,alpha,beta) PointChi2(prob,2.0(alpha))/(2.0(beta))
57	#define PAI2 6.283185307
58	#define TINY 1.0e-20
59	#define EVALUATE_COMPLEX_NUMBERS 2
60	#if !defined(MAX)
61	#define MAX(a,b) (((a) > (b)) ? (a) : (b))
62	#endif
63	#if !defined(MIN)
64	#define MIN(a,b) (((a) < (b)) ? (a) : (b))
65	#endif
66	#define SQUARE(a) ((a)*(a))
67
68	/* prototypes */
69	void AddTwoMatrices (int dim, MrBFlt a, MrBFlt b, MrBFlt **result);
70	void BackSubstitutionRow (int dim, MrBFlt *u, MrBFlt b);
71	void Balanc (int dim, MrBFlt *a, int low, int high, MrBFlt scale);
72	void BalBak (int dim, int low, int high, MrBFlt scale, int m, MrBFlt *z);
73	MrBFlt BetaCf (MrBFlt a, MrBFlt b, MrBFlt x);
74	MrBFlt BetaQuantile (MrBFlt alpha, MrBFlt beta, MrBFlt x);
75	MrBFlt CdfBinormal (MrBFlt h1, MrBFlt h2, MrBFlt r);
76	MrBFlt CdfNormal (MrBFlt x);
77	complex Complex (MrBFlt a, MrBFlt b);
78	MrBFlt ComplexAbsoluteValue (complex a);
79	complex ComplexAddition (complex a, complex b);
80	complex ComplexConjugate (complex a);
81	complex ComplexDivision (complex a, complex b);
82	void ComplexDivision2 (MrBFlt ar, MrBFlt ai, MrBFlt br, MrBFlt bi, MrBFlt cr, MrBFlt ci);
83	complex ComplexExponentiation (complex a);
84	int ComplexInvertMatrix (int dim, complex *a, MrBFlt dwork, int indx, complex aInverse, complex col);
85	complex ComplexLog (complex a);
86	void ComplexLUBackSubstitution (int dim, complex *a, int indx, complex *b);
87	int ComplexLUDecompose (int dim, complex *a, MrBFlt vv, int indx, MrBFlt pd);
88	complex ComplexMultiplication (complex a, complex b);
89	complex ComplexSquareRoot (complex a);
90	complex ComplexSubtraction (complex a, complex b);
91	int ComputeEigenSystem (int dim, MrBFlt *a, MrBFlt v, MrBFlt vi, MrBFlt u, int iwork, MrBFlt *dwork);
92	void ComputeLandU (int dim, MrBFlt aMat, MrBFlt lMat, MrBFlt **uMat);
93	void ComputeMatrixExponential (int dim, MrBFlt a, int qValue, MrBFlt f);
94	void DivideByTwos (int dim, MrBFlt **a, int power);
95	MrBFlt D_sign (MrBFlt a, MrBFlt b);
96	int EigensForRealMatrix (int dim, MrBFlt *a, MrBFlt wr, MrBFlt wi, MrBFlt z, int iv1, MrBFlt *fv1);
97	void ElmHes (int dim, int low, int high, MrBFlt *a, int interchanged);
98	void ElTran (int dim, int low, int high, MrBFlt *a, int interchanged, MrBFlt **z);
99	void Exchange (int j, int k, int l, int m, int n, MrBFlt *a, MrBFlt scale);
100	MrBFlt Factorial (int x);
101	void ForwardSubstitutionRow (int dim, MrBFlt *L, MrBFlt b);
102	MrBFlt GammaRandomVariable (MrBFlt a, MrBFlt b, SafeLong *seed);
103	void GaussianElimination (int dim, MrBFlt a, MrBFlt bMat, MrBFlt **xMat);
104	int Hqr2 (int dim, int low, int high, MrBFlt *h, MrBFlt wr, MrBFlt wi, MrBFlt *z);
105	MrBFlt IncompleteBetaFunction (MrBFlt alpha, MrBFlt beta, MrBFlt x);
106	MrBFlt IncompleteGamma (MrBFlt x, MrBFlt alpha, MrBFlt LnGamma_alpha);
107	int InvertMatrix (int dim, MrBFlt *a, MrBFlt col, int indx, MrBFlt *aInv);
108	MrBFlt LBinormal (MrBFlt h1, MrBFlt h2, MrBFlt r);
109	int LogBase2Plus1 (MrBFlt x);
110	void LUBackSubstitution (int dim, MrBFlt *a, int indx, MrBFlt *b);
111	int LUDecompose (int dim, MrBFlt *a, MrBFlt vv, int indx, MrBFlt pd);
112	void MultiplyMatrixByScalar (int dim, MrBFlt a, MrBFlt scalar, MrBFlt result);
113	MrBFlt PointChi2 (MrBFlt prob, MrBFlt v);
114	void PrintComplexVector (int dim, complex *vec);
115	void PrintSquareComplexMatrix (int dim, complex **m);
116	void PrintSquareDoubleMatrix (int dim, MrBFlt **matrix);
117	void PrintSquareIntegerMatrix (int dim, int **matrix);
118	complex ProductOfRealAndComplex (MrBFlt a, complex b);
119	MrBFlt RndGamma (MrBFlt s, SafeLong *seed);
120	MrBFlt RndGamma1 (MrBFlt s, SafeLong *seed);
121	MrBFlt RndGamma2 (MrBFlt s, SafeLong *seed);
122	int SetQvalue (MrBFlt tol);
123	void SetToIdentity (int dim, MrBFlt **matrix);
124	MrBFlt Tha (MrBFlt h1, MrBFlt h2, MrBFlt a1, MrBFlt a2);
125	void TiProbsUsingEigens (int dim, MrBFlt cijk, MrBFlt eigenVals, MrBFlt v, MrBFlt r, MrBFlt tMat, MrBFlt fMat, MrBFlt **sMat);
126	void TiProbsUsingPadeApprox (int dim, MrBFlt qMat, MrBFlt v, MrBFlt r, MrBFlt tMat, MrBFlt fMat, MrBFlt sMat);
127
128
129
130
131
132	/*---------------------------------------------------------------------------------
133	\|
134	\| AddTwoMatrices
135	\|
136	\| Takes the sum of two matrices, "a" and "b", and puts the results in a matrix
137	\| called "result".
138	\|
139	---------------------------------------------------------------------------------*/
140	void AddTwoMatrices (int dim, MrBFlt a, MrBFlt b, MrBFlt **result)
141
142	{
143
144	int row, col;
145
146	for (row=0; row<dim; row++)
147	{
148	for (col=0; col<dim; col++)
149	{
150	result[row][col] = a[row][col] + b[row][col];
151	}
152	}
153
154	}
155
156
157
158
159
160	/*---------------------------------------------------------------------------------
161	\|
162	\| AllocateSquareComplexMatrix
163	\|
164	\| Allocate memory for a square (dim X dim) complex matrix.
165	\|
166	---------------------------------------------------------------------------------*/
167	complex **AllocateSquareComplexMatrix (int dim)
168
169	{
170
171	int i;
172	complex **m;
173
174	m = (complex *) SafeMalloc((size_t)((dim)sizeof(complex*)));
175	if (!m)
176	{
177	MrBayesPrint ("%s Error: Problem allocating a square complex matrix.\n", spacer);
178	exit (0);
179	}
180	m[0]=(complex ) SafeMalloc((size_t)((dimdim)*sizeof(complex)));
181	if (!m[0])
182	{
183	MrBayesPrint ("%s Error: Problem allocating a square complex matrix.\n", spacer);
184	exit (0);
185	}
186	for(i=1;i<dim;i++)
187	{
188	m[i] = m[i-1] + dim;
189	}
190
191	return (m);
192
193	}
194
195
196
197
198
199
200	/*---------------------------------------------------------------------------------
201	\|
202	\| AllocateSquareDoubleMatrix
203	\|
204	\| Allocate memory for a square (dim X dim) matrix of doubles.
205	\|
206	---------------------------------------------------------------------------------*/
207	MrBFlt **AllocateSquareDoubleMatrix (int dim)
208
209	{
210
211	int i;
212	MrBFlt **m;
213
214	m = (MrBFlt *)SafeMalloc((size_t)((dim)sizeof(MrBFlt*)));
215	if (!m)
216	{
217	MrBayesPrint ("%s Error: Problem allocating a square matrix of doubles.\n", spacer);
218	exit(1);
219	}
220	m[0] = (MrBFlt )SafeMalloc((size_t)((dimdim)*sizeof(MrBFlt)));
221	if (!m[0])
222	{
223	MrBayesPrint ("%s Error: Problem allocating a square matrix of doubles.\n", spacer);
224	exit(1);
225	}
226	for(i=1; i<dim; i++)
227	{
228	m[i] = m[i-1] + dim;
229	}
230
231	return (m);
232
233	}
234
235
236
237
238
239	/*---------------------------------------------------------------------------------
240	\|
241	\| AllocateSquareIntegerMatrix
242	\|
243	\| Allocate memory for a square (dim X dim) matrix of integers.
244	\|
245	---------------------------------------------------------------------------------*/
246	int **AllocateSquareIntegerMatrix (int dim)
247
248	{
249
250	int i, **m;
251
252	m = (int *)SafeMalloc((size_t)((dim)sizeof(int*)));
253	if (!m)
254	{
255	MrBayesPrint ("%s Error: Problem allocating a square matrix of integers.\n", spacer);
256	exit(1);
257	}
258	m[0] = (int )SafeMalloc((size_t)((dimdim)*sizeof(int)));
259	if (!m[0])
260	{
261	MrBayesPrint ("%s Error: Problem allocating a square matrix of integers.\n", spacer);
262	exit(1);
263	}
264	for(i=1; i<dim; i++)
265	{
266	m[i] = m[i-1] + dim;
267	}
268
269	return (m);
270
271	}
272
273
274
275
276
277	/*---------------------------------------------------------------------------------
278	\|
279	\| AutodGamma
280	\|
281	\| Auto-discrete-gamma distribution of rates over sites, K equal-probable
282	\| categories, with the mean for each category used.
283	\| This routine calculates M[], using rho and K (numGammaCats)
284	\|
285	---------------------------------------------------------------------------------*/
286	int AutodGamma (MrBFlt *M, MrBFlt rho, int K)
287
288	{
289
290	int i, j, i1, i2;
291	MrBFlt point[MAX_GAMMA_CATS], x, y, large = 20.0, sum;
292
293	for (i=0; i<K-1; i++)
294	point[i] = PointNormal ((i + 1.0) / K);
295	for (i=0; i<K; i++)
296	{
297	for (j=0; j<K; j++)
298	{
299	x = (i < K-1 ? point[i]:large);
300	y = (j < K-1 ? point[j]:large);
301	M[i * K + j] = CdfBinormal (x, y, rho);
302	}
303	}
304	for (i1=0; i1<2*K-1; i1++)
305	{
306	for (i2=0; i2<K*K; i2++)
307	{
308	i = i2 / K;
309	j = i2 % K;
310	if (AreDoublesEqual(i+j, 2*(K-1.0)-i1, ETA)==NO)
311	continue;
312	y = 0;
313	if (i > 0)
314	y -= M[(i-1)*K+j];
315	if (j > 0)
316	y -= M[i*K+(j-1)];
317	if (i > 0 && j > 0)
318	y += M[(i-1)*K+(j-1)];
319	M[iK+j] = (M[iK+j] + y) * K;
320	}
321	}
322	for (i=0; i<K; i++)
323	{
324	sum = 0.0;
325	for (j=0; j<K; j++)
326	{
327	if (M[i*K+j] < 0.0)
328	M[i*K+j] = 0.0;
329	sum += M[i*K+j];
330	}
331	for (j=0; j<K; j++)
332	M[i*K+j] /= sum;
333	}
334
335	# if 0
336	MrBayesPrint ("rho = %lf\n", rho);
337	for (i=0; i<K; i++)
338	{
339	for (j=0; j<K; j++)
340	MrBayesPrint ("%lf ", M[i*K + j]);
341	MrBayesPrint ("\n");
342	}
343	# endif
344
345	return (NO_ERROR);
346
347	}
348
349
350
351
352
353	/*---------------------------------------------------------------------------------
354	\|
355	\| BackSubstitutionRow
356	\|
357	---------------------------------------------------------------------------------*/
358	void BackSubstitutionRow (int dim, MrBFlt *u, MrBFlt b)
359
360	{
361
362	int i, j;
363	MrBFlt dotProduct;
364
365	b[dim-1] /= u[dim-1][dim-1];
366	for (i=dim-2; i>=0; i--)
367	{
368	dotProduct = 0.0;
369	for (j=i+1; j<dim; j++)
370	dotProduct += u[i][j] * b[j];
371	b[i] = (b[i] - dotProduct) / u[i][i];
372	}
373
374	}
375
376
377
378
379
380	/*---------------------------------------------------------------------------------
381	\|
382	\| Balanc
383	\|
384	\| This subroutine balances a real matrix and isolates
385	\| eigenvalues whenever possible.
386	\|
387	\| On input:
388	\|
389	\| * dim is the order of the matrix
390	\|
391	\| * a contains the input matrix to be balanced
392	\|
393	\| On output:
394	\|
395	\| * a contains the balanced matrix.
396	\|
397	\| * low and high are two integers such that a(i,j)
398	\| is equal to zero if
399	\| (1) i is greater than j and
400	\| (2) j=1,...,low-1 or i=igh+1,...,n.
401	\|
402	\| * scale contains information determining the
403	\| permutations and scaling factors used.
404	\|
405	\| Suppose that the principal submatrix in rows pLow through pHigh
406	\| has been balanced, that p(j) denotes the index interchanged
407	\| with j during the permutation step, and that the elements
408	\| of the diagonal matrix used are denoted by d(i,j). Then
409	\| scale(j) = p(j), for j = 1,...,pLow-1
410	\| = d(j,j), j = pLow,...,pHigh
411	\| = p(j) j = pHigh+1,...,dim.
412	\| The order in which the interchanges are made is dim to pHigh+1,
413	\| then 1 to pLow-1.
414	\|
415	\| Note that 1 is returned for pHigh if pHigh is zero formally.
416	\|
417	\| The algol procedure exc contained in balance appears in
418	\| balanc in line. (Note that the algol roles of identifiers
419	\| k,l have been reversed.)
420	\|
421	\| This routine is a translation of the Algol procedure from
422	\| Handbook for Automatic Computation, vol. II, Linear Algebra,
423	\| by Wilkinson and Reinsch, Springer-Verlag.
424	\|
425	\| This function was converted from FORTRAN by D. L. Swofford.
426	\|
427	---------------------------------------------------------------------------------*/
428	void Balanc (int dim, MrBFlt *a, int low, int high, MrBFlt scale)
429
430	{
431
432	int i, j, k, l, m, noconv;
433	MrBFlt c, f, g, r, s, b2;
434
435	b2 = FLT_RADIX * FLT_RADIX;
436	k = 0;
437	l = dim - 1;
438
439	for (j=l; j>=0; j--)
440	{
441	for (i=0; i<=l; i++)
442	{
443	if (i != j)
444	{
445	if (AreDoublesEqual(a[j][i],0.0, ETA)==NO)
446	goto next_j1;
447	}
448	}
449
450	/* bug that DLS caught */
451	/*m = l;
452	Exchange(j, k, l, m, dim, a, scale);
453	if (l < 0)
454	goto leave;
455	else
456	j = --l;*/
457	m = l;
458	Exchange(j, k, l, m, dim, a, scale);
459	if (--l < 0)
460	goto leave;
461	next_j1:
462	;
463	}
464
465	for (j=k; j<=l; j++)
466	{
467	for (i=k; i<=l; i++)
468	{
469	if (i != j)
470	{
471	if (AreDoublesEqual(a[i][j], 0.0, ETA)==NO)
472	goto next_j;
473	}
474	}
475	m = k;
476	Exchange(j, k, l, m, dim, a, scale);
477	k++;
478	next_j:
479	;
480	}
481
482	for (i=k; i<=l; i++)
483	scale[i] = 1.0;
484
485	do {
486	noconv = FALSE;
487	for (i=k; i<=l; i++)
488	{
489	c = 0.0;
490	r = 0.0;
491	for (j=k; j<=l; j++)
492	{
493	if (j != i)
494	{
495	c += fabs(a[j][i]);
496	r += fabs(a[i][j]);
497	}
498	}
499	if (AreDoublesEqual(c,0.0,ETA)==NO && AreDoublesEqual(r,0.0,ETA)==NO)
500	{
501	g = r / FLT_RADIX;
502	f = 1.0;
503	s = c + r;
504	while (c < g)
505	{
506	f *= FLT_RADIX;
507	c *= b2;
508	}
509	g = r * FLT_RADIX;
510	while (c >= g)
511	{
512	f /= FLT_RADIX;
513	c /= b2;
514	}
515	if ((c + r) / f < s * .95)
516	{
517	g = 1.0 / f;
518	scale[i] *= f;
519	noconv = TRUE;
520	for (j=k; j<dim; j++)
521	a[i][j] *= g;
522	for (j=0; j<=l; j++)
523	a[j][i] *= f;
524	}
525	}
526	}
527	}
528	while (noconv);
529	leave:
530	*low = k;
531	*high = l;
532
533	# if 0
534	/* begin f2c version of code:
535	balanc.f -- translated by f2c (version 19971204) */
536	int balanc (int nm, int n, MrBFlt a, int low, int igh, MrBFlt scale)
537
538	{
539
540	/* System generated locals */
541	int a_dim1, a_offset, i__1, i__2;
542	MrBFlt d__1;
543
544	/* Local variables */
545	static MrBFlt iexc;
546	static MrBFlt c__, f, g;
547	static MrBFlt i__, j, k, l, m;
548	static MrBFlt r__, s, radix, b2;
549	static MrBFlt jj;
550	static logical noconv;
551
552	/* parameter adjustments */
553	--scale;
554	a_dim1 = *nm;
555	a_offset = a_dim1 + 1;
556	a -= a_offset;
557
558	/* function Body */
559	radix = 16.0;
560
561	b2 = radix * radix;
562	k = 1;
563	l = *n;
564	goto L100;
565
566	/* .......... in-line procedure for row and column exchange .......... */
567	L20:
568	scale[m] = (MrBFlt) j;
569	if (j == m)
570	goto L50;
571
572	i__1 = l;
573	for (i__ = 1; i__ <= i__1; ++i__)
574	{
575	f = a[i__ + j * a_dim1];
576	a[i__ + j * a_dim1] = a[i__ + m * a_dim1];
577	a[i__ + m * a_dim1] = f;
578	/* L30: */
579	}
580
581	i__1 = *n;
582	for (i__ = k; i__ <= i__1; ++i__)
583	{
584	f = a[j + i__ * a_dim1];
585	a[j + i__ * a_dim1] = a[m + i__ * a_dim1];
586	a[m + i__ * a_dim1] = f;
587	/* L40: */
588	}
589
590	L50:
591	switch (iexc)
592	{
593	case 1:
594	goto L80;
595	case 2:
596	goto L130;
597	}
598
599	/* .......... search for rows isolating an eigenvalue and push them down .......... */
600	L80:
601	if (l == 1)
602	goto L280;
603	--l;
604
605	/* .......... for j=l step -1 until 1 do -- .......... */
606	L100:
607	i__1 = l;
608	for (jj = 1; jj <= i__1; ++jj)
609	{
610	j = l + 1 - jj;
611	i__2 = l;
612	for (i__ = 1; i__ <= i__2; ++i__)
613	{
614	if (i__ == j)
615	goto L110;
616	if (a[j + i__ * a_dim1] != 0.)
617	goto L120;
618	L110:
619	;
620	}
621	m = l;
622	iexc = 1;
623	goto L20;
624	L120:
625	;
626	}
627
628	goto L140;
629	/* .......... search for columns isolating an eigenvalue and push them left .......... */
630	L130:
631	++k;
632
633	L140:
634	i__1 = l;
635	for (j = k; j <= i__1; ++j)
636	{
637	i__2 = l;
638	for (i__ = k; i__ <= i__2; ++i__)
639	{
640	if (i__ == j)
641	goto L150;
642	if (a[i__ + j * a_dim1] != 0.)
643	goto L170;
644	L150:
645	;
646	}
647	m = k;
648	iexc = 2;
649	goto L20;
650	L170:
651	;
652	}
653
654	/* .......... now balance the submatrix in rows k to l .......... */
655	i__1 = l;
656	for (i__ = k; i__ <= i__1; ++i__)
657	{
658	/* L180: */
659	scale[i__] = 1.0;
660	}
661	/* .......... iterative loop for norm reduction .......... */
662	L190:
663	noconv = FALSE;
664
665	i__1 = l;
666	for (i__ = k; i__ <= i__1; ++i__)
667	{
668	c__ = 0.0;
669	r__ = 0.0;
670	i__2 = l;
671	for (j = k; j <= i__2; ++j)
672	{
673	if (j == i__)
674	goto L200;
675	c__ += (d__1 = a[j + i__ * a_dim1], abs(d__1));
676	r__ += (d__1 = a[i__ + j * a_dim1], abs(d__1));
677	L200:
678	;
679	}
680
681	/* .......... guard against zero c or r due to underflow .......... */
682	if (c__ == 0. \|\| r__ == 0.)
683	goto L270;
684	g = r__ / radix;
685	f = 1.0;
686	s = c__ + r__;
687	L210:
688	if (c__ >= g)
689	goto L220;
690	f *= radix;
691	c__ *= b2;
692	goto L210;
693	L220:
694	g = r__ * radix;
695	L230:
696	if (c__ < g)
697	goto L240;
698	f /= radix;
699	c__ /= b2;
700	goto L230;
701
702	/* .......... now balance .......... */
703	L240:
704	if ((c__ + r__) / f >= s * .95)
705	goto L270;
706	g = 1.0 / f;
707	scale[i__] *= f;
708	noconv = TRUE;
709
710	i__2 = *n;
711	for (j = k; j <= i__2; ++j)
712	{
713	/* L250: */
714	a[i__ + j * a_dim1] *= g;
715	}
716
717	i__2 = l;
718	for (j = 1; j <= i__2; ++j)
719	{
720	/* L260: */
721	a[j + i__ * a_dim1] *= f;
722	}
723
724	L270:
725	;
726	}
727
728	if (noconv)
729	goto L190;
730
731	L280:
732	*low = k;
733	*igh = l;
734	return 0;
735
736	}
737	/* end f2c version of code */
738	# endif
739
740	}
741
742
743
744
745
746	/*---------------------------------------------------------------------------------
747	\|
748	\| BalBak
749	\|
750	\| This subroutine forms the eigenvectors of a real general
751	\| matrix by back transforming those of the corresponding
752	\| balanced matrix determined by balance.
753	\|
754	\| On input:
755	\|
756	\| * dim is the order of the matrix
757	\|
758	\| * low and high are integers determined by balance
759	\|
760	\| * scale contains information determining the permutations
761	\| and scaling factors used by balance
762	\|
763	\| * m is the number of columns of z to be back transformed
764	\|
765	\| * z contains the real and imaginary parts of the eigen-
766	\| vectors to be back transformed in its first m columns
767	\|
768	\| On output:
769	\|
770	\| * z contains the real and imaginary parts of the
771	\| transformed eigenvectors in its first m columns
772	\|
773	\| This routine is a translation of the Algol procedure from
774	\| Handbook for Automatic Computation, vol. II, Linear Algebra,
775	\| by Wilkinson and Reinsch, Springer-Verlag.
776	\|
777	---------------------------------------------------------------------------------*/
778	void BalBak (int dim, int low, int high, MrBFlt scale, int m, MrBFlt *z)
779
780	{
781
782	int i, j, k, ii;
783	MrBFlt s;
784
785	if (m != 0) /* change "==" to "!=" to eliminate a goto statement */
786	{
787	if (high != low) /* change "==" to "!=" to eliminate a goto statement */
788	{
789	for (i=low; i<=high; i++)
790	{
791	s = scale[i];
792	for (j=0; j<m; j++)
793	z[i][j] *= s;
794	}
795	}
796	for (ii=0; ii<dim; ii++)
797	{
798	i = ii;
799	if ((i < low) \|\| (i > high)) /* was (i >= lo) && (i<= hi) but this */
800	{ /* eliminates a goto statement */
801	if (i < low)
802	i = low - ii;
803	k = (int)scale[i];
804	if (k != i) /* change "==" to "!=" to eliminate a goto statement */
805	{
806	for (j = 0; j < m; j++)
807	{
808	s = z[i][j];
809	z[i][j] = z[k][j];
810	z[k][j] = s;
811	}
812	}
813	}
814	}
815	}
816
817	#if 0
818	/* begin f2c version of code:
819	balbak.f -- translated by f2c (version 19971204) */
820	int balbak (int nm, int n, int low, int igh, MrBFlt scale, int m, MrBFlt *z__)
821
822	{
823
824	/* system generated locals */
825	int z_dim1, z_offset, i__1, i__2;
826
827	/* Local variables */
828	static int i__, j, k;
829	static MrBFlt s;
830	static int ii;
831
832	/* parameter adjustments */
833	--scale;
834	z_dim1 = *nm;
835	z_offset = z_dim1 + 1;
836	z__ -= z_offset;
837
838	/* function Body */
839	if (*m == 0)
840	goto L200;
841	if (igh == low)
842	goto L120;
843
844	i__1 = *igh;
845	for (i__ = *low; i__ <= i__1; ++i__)
846	{
847	s = scale[i__];
848	/* .......... left hand eigenvectors are back transformed */
849	/* if the foregoing statement is replaced by */
850	/* s=1.0d0/scale(i) ........... */
851	i__2 = *m;
852	for (j = 1; j <= i__2; ++j)
853	{
854	/* L100: */
855	z__[i__ + j * z_dim1] *= s;
856	}
857
858	/* L110: */
859	}
860
861	/* .........for i=low-1 step -1 until 1, igh+1 step 1 until n do -- .......... */
862	L120:
863	i__1 = *n;
864	for (ii = 1; ii <= i__1; ++ii)
865	{
866	i__ = ii;
867	if (i__ >= low && i__ <= igh)
868	goto L140;
869	if (i__ < *low)
870	i__ = *low - ii;
871	k = (integer) scale[i__];
872	if (k == i__)
873	goto L140;
874
875	i__2 = *m;
876	for (j = 1; j <= i__2; ++j)
877	{
878	s = z__[i__ + j * z_dim1];
879	z__[i__ + j * z_dim1] = z__[k + j * z_dim1];
880	z__[k + j * z_dim1] = s;
881	/* L130: */
882	}
883	L140:
884	;
885	}
886
887	L200:
888	return 0;
889
890	}
891	/* end f2c version of code */
892	#endif
893
894	}
895
896
897
898
899
900	void BetaBreaks (MrBFlt alpha, MrBFlt beta, MrBFlt *values, int K)
901
902	{
903
904	int i;
905	MrBFlt r, quantile, lower, upper;
906
907	r = (1.0 / K) * 0.5;
908	lower = 0.0;
909	upper = (1.0 / K);
910	r = (upper - lower) * 0.5 + lower;
911	for (i=0; i<K; i++)
912	{
913	quantile = BetaQuantile (alpha, beta, r);
914	values[i] = quantile;
915	lower += (1.0/K);
916	upper += (1.0/K);
917	r += (1.0/K);
918	}
919
920	# if 0
921	for (i=0; i<K; i++)
922	{
923	MrBayesPrint ("%4d %lf %lf\n", i, values[i]);
924	}
925	# endif
926
927	}
928
929
930
931
932
933	MrBFlt BetaCf (MrBFlt a, MrBFlt b, MrBFlt x)
934
935	{
936
937	int m, m2;
938	MrBFlt aa, c, d, del, h, qab, qam, qap;
939
940	qab = a + b;
941	qap = a + 1.0;
942	qam = a - 1.0;
943	c = 1.0;
944	d = 1.0 - qab * x / qap;
945	if (fabs(d) < (1.0e-30))
946	d = (1.0e-30);
947	d = 1.0 / d;
948	h = d;
949	for (m=1; m<=100; m++)
950	{
951	m2 = 2 * m;
952	aa = m * (b-m) * x / ((qam+m2) * (a+m2));
953	d = 1.0 + aa * d;
954	if (fabs(d) < (1.0e-30))
955	d = (1.0e-30);
956	c = 1.0 + aa / c;
957	if (fabs(c) < (1.0e-30))
958	c = (1.0e-30);
959	d = 1.0 / d;
960	h = d c;
961	aa = -(a+m) * (qab+m) * x / ((a+m2) * (qap+m2));
962	d = 1.0 + aa * d;
963	if (fabs(d) < (1.0e-30))
964	d = (1.0e-30);
965	c = 1.0 + aa / c;
966	if (fabs(c) < (1.0e-30))
967	c = (1.0e-30);
968	d = 1.0 / d;
969	del = d * c;
970	h *= del;
971	if (fabs(del - 1.0) < (3.0e-7))
972	break;
973	}
974	if (m > 100)
975	{
976	MrBayesPrint ("%s Error in BetaCf.\n", spacer);
977	exit(0);
978	}
979	return (h);
980
981	}
982
983
984
985
986
987	MrBFlt BetaQuantile (MrBFlt alpha, MrBFlt beta, MrBFlt x)
988
989	{
990
991	int i, stopIter, direction, nswitches;
992	MrBFlt curPos, curFraction, increment;
993
994	i = nswitches = 0;
995	curPos = 0.5;
996	stopIter = NO;
997	increment = 0.25;
998	curFraction = IncompleteBetaFunction (alpha, beta, curPos);
999	if (curFraction > x)
1000	direction = DOWN;
1001	else
1002	direction = UP;
1003
1004	while (stopIter == NO)
1005	{
1006	curFraction = IncompleteBetaFunction (alpha, beta, curPos);
1007	if (curFraction > x && direction == DOWN)
1008	{
1009	/* continue going down */
1010	while (curPos - increment <= 0.0)
1011	{
1012	increment /= 2.0;
1013	}
1014	curPos -= increment;
1015	}
1016	else if (curFraction > x && direction == UP)
1017	{
1018	/* switch directions, and go down */
1019	nswitches++;
1020	direction = DOWN;
1021	while (curPos - increment <= 0.0)
1022	{
1023	increment /= 2.0;
1024	}
1025	increment /= 2.0;
1026	curPos -= increment;
1027	}
1028	else if (curFraction < x && direction == UP)
1029	{
1030	/* continue going up */
1031	while (curPos + increment >= 1.0)
1032	{
1033	increment /= 2.0;
1034	}
1035	curPos += increment;
1036	}
1037	else if (curFraction < x && direction == DOWN)
1038	{
1039	/* switch directions, and go up */
1040	nswitches++;
1041	direction = UP;
1042	while (curPos + increment >= 1.0)
1043	{
1044	increment /= 2.0;
1045	}
1046	increment /= 2.0;
1047	curPos += increment;
1048	}
1049	else
1050	{
1051	stopIter = YES;
1052	}
1053	if (i > 1000 \|\| nswitches > 20)
1054	stopIter = YES;
1055	i++;
1056	}
1057
1058	return (curPos);
1059
1060	}
1061
1062
1063
1064
1065
1066	/*---------------------------------------------------------------------------------
1067	\|
1068	\| CalcCijk
1069	\|
1070	\| This function precalculates the product of the eigenvectors and their
1071	\| inverse for faster calculation of transition probabilities. The output
1072	\| is a vector of precalculated values. The input is the eigenvectors (u) and
1073	\| the inverse of the eigenvector matrix (v).
1074	\|
1075	---------------------------------------------------------------------------------*/
1076	void CalcCijk (int dim, MrBFlt c_ijk, MrBFlt u, MrBFlt *v)
1077
1078	{
1079
1080	register int i, j, k;
1081	MrBFlt *pc;
1082
1083	pc = c_ijk;
1084	for (i=0; i<dim; i++)
1085	for (j=0; j<dim; j++)
1086	for (k=0; k<dim; k++)
1087	pc++ = u[i][k] v[k][j];
1088
1089	}
1090
1091
1092
1093
1094
1095	/*---------------------------------------------------------------------------------
1096	\|
1097	\| CdfBinormal
1098	\|
1099	\| F(h1,h2,r) = prob(x<h1, y<h2), where x and y are standard binormal.
1100	\|
1101	---------------------------------------------------------------------------------*/
1102	MrBFlt CdfBinormal (MrBFlt h1, MrBFlt h2, MrBFlt r)
1103
1104	{
1105
1106	return (LBinormal(h1, h2, r) + CdfNormal(h1) + CdfNormal(h2) - 1.0);
1107
1108	}
1109
1110
1111
1112
1113
1114	/*---------------------------------------------------------------------------------
1115	\|
1116	\| CdfNormal
1117	\|
1118	\| Calculates the cumulative density distribution (CDF) for the normal using:
1119	\|
1120	\| Hill, I. D. 1973. The normal integral. Applied Statistics, 22:424-427.
1121	\| (AS66)
1122	\|
1123	---------------------------------------------------------------------------------*/
1124	MrBFlt CdfNormal (MrBFlt x)
1125
1126	{
1127
1128	int invers = 0;
1129	MrBFlt p, limit = 10.0, t = 1.28, y = x*x/2.0;
1130
1131	if (x < 0.0)
1132	{
1133	invers = 1;
1134	x *= -1.0;
1135	}
1136	if (x > limit)
1137	return (invers ? 0 : 1);
1138	if (x < t)
1139	p = 0.5 - x * (0.398942280444 - 0.399903438504 * y /
1140	(y + 5.75885480458 - 29.8213557808 /
1141	(y + 2.62433121679 + 48.6959930692 /
1142	(y + 5.92885724438))));
1143	else
1144	p = 0.398942280385 * exp(-y) /
1145	(x - 3.8052e-8 + 1.00000615302 /
1146	(x + 3.98064794e-4 + 1.98615381364 /
1147	(x - 0.151679116635 + 5.29330324926 /
1148	(x + 4.8385912808 - 15.1508972451 /
1149	(x + 0.742380924027 + 30.789933034 /
1150	(x + 3.99019417011))))));
1151
1152	return (invers ? p : 1-p);
1153
1154	}
1155
1156
1157
1158
1159
1160	/*---------------------------------------------------------------------------------
1161	\|
1162	\| Complex
1163	\|
1164	\| Returns a complex number with specified real and imaginary parts.
1165	\|
1166	---------------------------------------------------------------------------------*/
1167	complex Complex (MrBFlt a, MrBFlt b)
1168
1169	{
1170
1171	complex c;
1172
1173	c.re = a;
1174	c.im = b;
1175
1176	return (c);
1177
1178	}
1179
1180
1181
1182
1183
1184	/*---------------------------------------------------------------------------------
1185	\|
1186	\| ComplexAbsoluteValue
1187	\|
1188	\| Returns the complex absolute value (modulus) of a complex number.
1189	\|
1190	---------------------------------------------------------------------------------*/
1191	MrBFlt ComplexAbsoluteValue (complex a)
1192
1193	{
1194
1195	MrBFlt x, y, answer, temp;
1196
1197	x = fabs(a.re);
1198	y = fabs(a.im);
1199	if(AreDoublesEqual(x, 0.0, ETA)==YES) /* x == 0.0 */
1200	answer = y;
1201	else if (AreDoublesEqual(y, 0.0, ETA)==YES) /* y == 0.0 */
1202	answer = x;
1203	else if (x > y)
1204	{
1205	temp = y / x;
1206	answer = x * sqrt(1.0 + temp * temp);
1207	}
1208	else
1209	{
1210	temp = x / y;
1211	answer = y * sqrt(1.0 + temp * temp);
1212	}
1213
1214	return (answer);
1215
1216	}
1217
1218
1219
1220
1221
1222	/*---------------------------------------------------------------------------------
1223	\|
1224	\| ComplexAddition
1225	\|
1226	\| Returns the complex sum of two complex numbers.
1227	\|
1228	---------------------------------------------------------------------------------*/
1229	complex ComplexAddition (complex a, complex b)
1230
1231	{
1232
1233	complex c;
1234
1235	c.re = a.re + b.re;
1236	c.im = a.im + b.im;
1237
1238	return (c);
1239
1240	}
1241
1242
1243
1244
1245
1246	/*---------------------------------------------------------------------------------
1247	\|
1248	\| ComplexConjugate
1249	\|
1250	\| Returns the complex conjugate of a complex number.
1251	\|
1252	---------------------------------------------------------------------------------*/
1253	complex ComplexConjugate (complex a)
1254
1255	{
1256
1257	complex c;
1258
1259	c.re = a.re;
1260	c.im = -a.im;
1261
1262	return (c);
1263
1264	}
1265
1266
1267
1268
1269
1270	/*---------------------------------------------------------------------------------
1271	\|
1272	\| ComplexDivision
1273	\|
1274	\| Returns the complex quotient of two complex numbers.
1275	\|
1276	---------------------------------------------------------------------------------*/
1277	complex ComplexDivision (complex a, complex b)
1278
1279	{
1280
1281	complex c;
1282	MrBFlt r, den;
1283
1284	if(fabs(b.re) >= fabs(b.im))
1285	{
1286	r = b.im / b.re;
1287	den = b.re + r * b.im;
1288	c.re = (a.re + r * a.im) / den;
1289	c.im = (a.im - r * a.re) / den;
1290	}
1291	else
1292	{
1293	r = b.re / b.im;
1294	den = b.im + r * b.re;
1295	c.re = (a.re * r + a.im) / den;
1296	c.im = (a.im * r - a.re) / den;
1297	}
1298
1299	return (c);
1300
1301	}
1302
1303
1304
1305
1306
1307	/*---------------------------------------------------------------------------------
1308	\|
1309	\| ComplexDivision2
1310	\|
1311	\| Returns the complex quotient of two complex numbers. It does not require that
1312	\| the numbers be in a complex structure.
1313	\|
1314	---------------------------------------------------------------------------------*/
1315	void ComplexDivision2 (MrBFlt ar, MrBFlt ai, MrBFlt br, MrBFlt bi, MrBFlt cr, MrBFlt ci)
1316
1317	{
1318
1319	MrBFlt s, ais, bis, ars, brs;
1320
1321	s = fabs(br) + fabs(bi);
1322	ars = ar / s;
1323	ais = ai / s;
1324	brs = br / s;
1325	bis = bi / s;
1326	s = brsbrs + bisbis;
1327	cr = (arsbrs + ais*bis) / s;
1328	ci = (aisbrs - ars*bis) / s;
1329
1330	}
1331
1332
1333
1334
1335
1336	/*---------------------------------------------------------------------------------
1337	\|
1338	\| ComplexExponentiation
1339	\|
1340	\| Returns the complex exponential of a complex number.
1341	\|
1342	---------------------------------------------------------------------------------*/
1343	complex ComplexExponentiation (complex a)
1344
1345	{
1346
1347	complex c;
1348
1349	c.re = exp(a.re);
1350	if (AreDoublesEqual(a.im,0.0, ETA)==YES) /* == 0 */
1351	c.im = 0;
1352	else
1353	{
1354	c.im = c.re*sin(a.im);
1355	c.re *= cos(a.im);
1356	}
1357
1358	return (c);
1359
1360	}
1361
1362
1363
1364
1365	/*---------------------------------------------------------------------------------
1366	\|
1367	\| ComplexInvertMatrix
1368	\|
1369	\| Inverts a matrix of complex numbers using the LU-decomposition method.
1370	\| The program has the following variables:
1371	\|
1372	\| a -- the matrix to be inverted
1373	\| aInverse -- the results of the matrix inversion
1374	\| dim -- the dimension of the square matrix a and its inverse
1375	\| dwork -- a work vector of doubles
1376	\| indx -- a work vector of integers
1377	\| col -- carries the results of the back substitution
1378	\|
1379	\| The function returns YES (1) or NO (0) if the results are singular.
1380	\|
1381	---------------------------------------------------------------------------------*/
1382	int ComplexInvertMatrix (int dim, complex *a, MrBFlt dwork, int indx, complex aInverse, complex col)
1383
1384	{
1385
1386	int isSingular, i, j;
1387
1388	isSingular = ComplexLUDecompose (dim, a, dwork, indx, (MrBFlt *)NULL);
1389
1390	if (isSingular == 0)
1391	{
1392	for (j=0; j<dim; j++)
1393	{
1394	for (i=0; i<dim; i++)
1395	col[i] = Complex (0.0, 0.0);
1396	col[j] = Complex (1.0, 0.0);
1397	ComplexLUBackSubstitution (dim, a, indx, col);
1398	for (i=0; i<dim; i++)
1399	aInverse[i][j] = col[i];
1400	}
1401	}
1402
1403	return (isSingular);
1404
1405	}
1406
1407
1408
1409
1410
1411	/*---------------------------------------------------------------------------------
1412	\|
1413	\| ComplexExponentiation
1414	\|
1415	\| Returns the complex exponential of a complex number.
1416	\|
1417	---------------------------------------------------------------------------------*/
1418	complex ComplexLog (complex a)
1419
1420	{
1421
1422	complex c;
1423
1424	c.re = log(ComplexAbsoluteValue(a));
1425	if (AreDoublesEqual(a.re,0.0,ETA)==YES) /* == 0.0 */
1426	{
1427	c.im = PIOVER2;
1428	}
1429	else
1430	{
1431	c.im = atan2(a.im, a.re);
1432	}
1433
1434	return (c);
1435
1436	}
1437
1438
1439
1440
1441
1442
1443	/*---------------------------------------------------------------------------------
1444	\|
1445	\| ComplexLUBackSubstitution
1446	\|
1447	\| Perform back-substitution into a LU-decomposed matrix to obtain
1448	\| the inverse.
1449	\|
1450	---------------------------------------------------------------------------------*/
1451	void ComplexLUBackSubstitution (int dim, complex *a, int indx, complex *b)
1452
1453	{
1454
1455	int i, ip, j, ii = -1;
1456	complex sum;
1457
1458	for (i = 0; i < dim; i++)
1459	{
1460	ip = indx[i];
1461	sum = b[ip];
1462	b[ip] = b[i];
1463	if (ii >= 0)
1464	{
1465	for (j = ii; j <= i - 1; j++)
1466	sum = ComplexSubtraction (sum, ComplexMultiplication (a[i][j], b[j]));
1467	}
1468	else if (AreDoublesEqual(sum.re,0.0,ETA)==NO \|\| AreDoublesEqual(sum.im, 0.0, ETA)==NO) /* 2x != 0.0 */
1469	ii = i;
1470	b[i] = sum;
1471	}
1472	for (i = dim - 1; i >= 0; i--)
1473	{
1474	sum = b[i];
1475	for (j = i + 1; j < dim; j++)
1476	sum = ComplexSubtraction (sum, ComplexMultiplication (a[i][j], b[j]));
1477	b[i] = ComplexDivision (sum, a[i][i]);
1478	}
1479
1480	}
1481
1482
1483
1484
1485
1486
1487	/*---------------------------------------------------------------------------------
1488	\|
1489	\| ComplexLUDecompose
1490	\|
1491	\| Replaces the matrix a with its LU-decomposition.
1492	\| The program has the following variables:
1493	\|
1494	\| a -- the matrix
1495	\| dim -- the dimension of the square matrix a and its inverse
1496	\| vv -- a work vector of doubles
1497	\| indx -- row permutation according to partitial pivoting sequence
1498	\| pd -- 1 if number of row interchanges was even, -1 if number of
1499	\| row interchanges was odd. Can be NULL.
1500	\|
1501	\| The function returns YES (1) or NO (0) if the results are singular.
1502	\|
1503	---------------------------------------------------------------------------------*/
1504	int ComplexLUDecompose (int dim, complex *a, MrBFlt vv, int indx, MrBFlt pd)
1505
1506	{
1507
1508	int i, imax, j, k;
1509	MrBFlt big, dum, temp, d;
1510	complex sum, cdum;
1511
1512	d = 1.0;
1513	imax = 0;
1514
1515	for (i = 0; i < dim; i++)
1516	{
1517	big = 0.0;
1518	for (j = 0; j < dim; j++)
1519	{
1520	if ((temp = ComplexAbsoluteValue (a[i][j])) > big)
1521	big = temp;
1522	}
1523	if (AreDoublesEqual(big, 0.0, ETA)==YES) /* == 0.0 */
1524	{
1525	MrBayesPrint ("%s Error: Problem in ComplexLUDecompose\n", spacer);
1526	return (1);
1527	}
1528	vv[i] = 1.0 / big;
1529	}
1530
1531	for (j = 0; j < dim; j++)
1532	{
1533	for (i = 0; i < j; i++)
1534	{
1535	sum = a[i][j];
1536	for (k = 0; k < i; k++)
1537	sum = ComplexSubtraction (sum, ComplexMultiplication (a[i][k], a[k][j]));
1538	a[i][j] = sum;
1539	}
1540	big = 0.0;
1541	for (i = j; i < dim; i++)
1542	{
1543	sum = a[i][j];
1544	for (k = 0; k < j; k++)
1545	sum = ComplexSubtraction (sum, ComplexMultiplication (a[i][k], a[k][j]));
1546	a[i][j] = sum;
1547	dum = vv[i] * ComplexAbsoluteValue (sum);
1548	if (dum >= big)
1549	{
1550	big = dum;
1551	imax = i;
1552	}
1553	}
1554	if (j != imax)
1555	{
1556	for (k = 0; k < dim; k++)
1557	{
1558	cdum = a[imax][k];
1559	a[imax][k] = a[j][k];
1560	a[j][k] = cdum;
1561	}
1562	d = -d;
1563	vv[imax] = vv[j];
1564	}
1565	indx[j] = imax;
1566	if (AreDoublesEqual(a[j][j].re, 0.0, ETA)==YES && AreDoublesEqual(a[j][j].im, 0.0, ETA)==YES) /* 2x == 0.0 */
1567	a[j][j] = Complex (1.0e-20, 1.0e-20);
1568	if (j != dim - 1)
1569	{
1570	cdum = ComplexDivision (Complex(1.0, 0.0), a[j][j]);
1571	for (i = j + 1; i < dim; i++)
1572	a[i][j] = ComplexMultiplication (a[i][j], cdum);
1573	}
1574	}
1575
1576	if (pd != NULL)
1577	*pd = d;
1578
1579	return (0);
1580
1581	}
1582
1583
1584
1585
1586
1587	/*---------------------------------------------------------------------------------
1588	\|
1589	\| ComplexMultiplication
1590	\|
1591	\| Returns the complex product of two complex numbers.
1592	\|
1593	---------------------------------------------------------------------------------*/
1594	complex ComplexMultiplication (complex a, complex b)
1595
1596	{
1597
1598	complex c;
1599
1600	c.re = a.re * b.re - a.im * b.im;
1601	c.im = a.im * b.re + a.re * b.im;
1602
1603	return (c);
1604
1605	}
1606
1607
1608
1609
1610
1611	/*---------------------------------------------------------------------------------
1612	\|
1613	\| ComplexSquareRoot
1614	\|
1615	\| Returns the complex square root of a complex number.
1616	\|
1617	---------------------------------------------------------------------------------*/
1618	complex ComplexSquareRoot (complex a)
1619
1620	{
1621
1622	complex c;
1623	MrBFlt x, y, w, r;
1624
1625	if (AreDoublesEqual(a.re, 0.0, ETA)==YES && AreDoublesEqual(a.im, 0.0, ETA)==YES) /* 2x == 0.0 */
1626	{
1627	c.re = 0.0;
1628	c.im = 0.0;
1629	return (c);
1630	}
1631	else
1632	{
1633	x = fabs(a.re);
1634	y = fabs(a.im);
1635	if (x >= y)
1636	{
1637	r = y / x;
1638	w = sqrt(x) * sqrt(0.5 * (1.0 + sqrt(1.0 + r * r)));
1639	}
1640	else
1641	{
1642	r = x / y;
1643	w = sqrt(y) * sqrt(0.5 * (r + sqrt(1.0 + r * r)));
1644	}
1645	if (a.re >= 0.0)
1646	{
1647	c.re = w;
1648	c.im = a.im / (2.0 * w);
1649	}
1650	else
1651	{
1652	c.im = (a.im >= 0.0) ? w : -w;
1653	c.re = a.im / (2.0 * c.im);
1654	}
1655	return (c);
1656	}
1657
1658	}
1659
1660
1661
1662
1663
1664	/*---------------------------------------------------------------------------------
1665	\|
1666	\| ComplexSubtraction
1667	\|
1668	\| Returns the complex difference of two complex numbers.
1669	\|
1670	---------------------------------------------------------------------------------*/
1671	complex ComplexSubtraction (complex a, complex b)
1672
1673	{
1674
1675	complex c;
1676
1677	c.re = a.re - b.re;
1678	c.im = a.im - b.im;
1679
1680	return (c);
1681
1682	}
1683
1684
1685
1686
1687
1688	/*---------------------------------------------------------------------------------
1689	\|
1690	\| ComputeEigenSystem
1691	\|
1692	\| Calculates the eigenvalues, eigenvectors, and the inverse of the eigenvectors
1693	\| for a matrix of real numbers.
1694	\|
1695	---------------------------------------------------------------------------------*/
1696	int ComputeEigenSystem (int dim, MrBFlt *a, MrBFlt v, MrBFlt vi, MrBFlt u, int iwork, MrBFlt *dwork)
1697
1698	{
1699
1700	int i, rc;
1701
1702	rc = EigensForRealMatrix (dim, a, v, vi, u, iwork, dwork);
1703	if (rc != NO_ERROR)
1704	{
1705	MrBayesPrint ("%s Error in ComputeEigenSystem.\n", spacer);
1706	return (ERROR);
1707	}
1708	for (i=0; i<dim; i++)
1709	{
1710	if (AreDoublesEqual(vi[i], 0.0, ETA)==NO) /* != 0.0 */
1711	return (EVALUATE_COMPLEX_NUMBERS);
1712	}
1713
1714	return (NO_ERROR);
1715
1716	}
1717
1718
1719
1720
1721
1722	/*---------------------------------------------------------------------------------
1723	\|
1724	\| ComputeLandU
1725	\|
1726	\| This function computes the L and U decomposition of a matrix. Basically,
1727	\| we find matrices lMat and uMat such that
1728	\|
1729	\| lMat * uMat = aMat
1730	\|
1731	---------------------------------------------------------------------------------*/
1732	void ComputeLandU (int dim, MrBFlt aMat, MrBFlt lMat, MrBFlt **uMat)
1733
1734	{
1735
1736	int i, j, k, m, row, col;
1737
1738	for (j=0; j<dim; j++)
1739	{
1740	for (k=0; k<j; k++)
1741	for (i=k+1; i<j; i++)
1742	aMat[i][j] = aMat[i][j] - aMat[i][k] * aMat[k][j];
1743
1744	for (k=0; k<j; k++)
1745	for (i=j; i<dim; i++)
1746	aMat[i][j] = aMat[i][j] - aMat[i][k]*aMat[k][j];
1747
1748	for (m=j+1; m<dim; m++)
1749	aMat[m][j] /= aMat[j][j];
1750	}
1751
1752	for (row=0; row<dim; row++)
1753	{
1754	for (col=0; col<dim; col++)
1755	{
1756	if (row <= col)
1757	{
1758	uMat[row][col] = aMat[row][col];
1759	lMat[row][col] = (row == col ? 1.0 : 0.0);
1760	}
1761	else
1762	{
1763	lMat[row][col] = aMat[row][col];
1764	uMat[row][col] = 0.0;
1765	}
1766	}
1767	}
1768
1769	}
1770
1771
1772
1773
1774
1775	/*---------------------------------------------------------------------------------
1776	\|
1777	\| ComputeMatrixExponential
1778	\|
1779	\| The method approximates the matrix exponential, f = e^a, using
1780	\| the algorithm 11.3.1, described in:
1781	\|
1782	\| Golub, G. H., and C. F. Van Loan. 1996. Matrix Computations, Third Edition.
1783	\| The Johns Hopkins University Press, Baltimore, Maryland.
1784	\|
1785	\| The method has the advantage of error control. The error is controlled by
1786	\| setting qValue appropriately (using the function SetQValue).
1787	\|
1788	---------------------------------------------------------------------------------*/
1789	void ComputeMatrixExponential (int dim, MrBFlt a, int qValue, MrBFlt f)
1790
1791	{
1792
1793	int i, j, k, negativeFactor;
1794	MrBFlt maxAValue, c, d, n, x, cX;
1795
1796	d = AllocateSquareDoubleMatrix (dim);
1797	n = AllocateSquareDoubleMatrix (dim);
1798	x = AllocateSquareDoubleMatrix (dim);
1799	cX = AllocateSquareDoubleMatrix (dim);
1800
1801	SetToIdentity (dim, d);
1802	SetToIdentity (dim, n);
1803	SetToIdentity (dim, x);
1804
1805	maxAValue = 0;
1806	for (i=0; i<dim; i++)
1807	maxAValue = MAX (maxAValue, a[i][i]);
1808
1809	j = MAX (0, LogBase2Plus1 (maxAValue));
1810
1811	DivideByTwos (dim, a, j);
1812
1813	c = 1;
1814	for (k=1; k<=qValue; k++)
1815	{
1816	c = c * (qValue - k + 1.0) / ((2.0 * qValue - k + 1.0) * k);
1817
1818	/* X = AX */
1819	MultiplyMatrices (dim, a, x, x);
1820
1821	/* N = N + cX */
1822	MultiplyMatrixByScalar (dim, x, c, cX);
1823	AddTwoMatrices (dim, n, cX, n);
1824
1825	/* D = D + (-1)^kcX /
1826	negativeFactor = (k % 2 == 0 ? 1 : -1);
1827	if (negativeFactor == -1)
1828	MultiplyMatrixByScalar (dim, cX, negativeFactor, cX);
1829	AddTwoMatrices (dim, d, cX, d);
1830	}
1831
1832	GaussianElimination (dim, d, n, f);
1833
1834	for (k = 0; k < j; k++)
1835	MultiplyMatrices (dim, f, f, f);
1836
1837	for (i=0; i<dim; i++)
1838	{
1839	for (j=0; j<dim; j++)
1840	{
1841	if (f[i][j] < 0.0)
1842	f[i][j] = fabs(f[i][j]);
1843	}
1844	}
1845
1846	FreeSquareDoubleMatrix (d);
1847	FreeSquareDoubleMatrix (n);
1848	FreeSquareDoubleMatrix (x);
1849	FreeSquareDoubleMatrix (cX);
1850
1851	}
1852
1853
1854
1855
1856
1857	/*---------------------------------------------------------------------------------
1858	\|
1859	\| CopyComplexMatrices
1860	\|
1861	\| Copies the contents of one matrix of complex numbers to another matrix.
1862	\|
1863	---------------------------------------------------------------------------------*/
1864	void CopyComplexMatrices (int dim, complex from, complex to)
1865
1866	{
1867
1868	int i, j;
1869
1870	for (i=0; i<dim; i++)
1871	{
1872	for (j=0; j<dim; j++)
1873	{
1874	to[i][j].re = from[i][j].re;
1875	to[i][j].im = from[i][j].im;
1876	}
1877	}
1878
1879	}
1880
1881
1882
1883
1884
1885	/*---------------------------------------------------------------------------------
1886	\|
1887	\| CopyDoubleMatrices
1888	\|
1889	\| Copies the contents of one matrix of doubles to another matrix.
1890	\|
1891	---------------------------------------------------------------------------------*/
1892	void CopyDoubleMatrices (int dim, MrBFlt from, MrBFlt to)
1893
1894	{
1895
1896	int i, j;
1897
1898	for (i=0; i<dim; i++)
1899	{
1900	for (j=0; j<dim; j++)
1901	{
1902	to[i][j] = from[i][j];
1903	}
1904	}
1905
1906	}
1907
1908
1909
1910
1911
1912	/*---------------------------------------------------------------------------------
1913	\|
1914	\| DirichletRandomVariable
1915	\|
1916	\| Generate a Dirichlet-distributed random variable. The parameter of the
1917	\| Dirichlet is contained in the vector alp. The random variable is contained
1918	\| in the vector z.
1919	\|
1920	---------------------------------------------------------------------------------*/
1921	void DirichletRandomVariable (MrBFlt alp, MrBFlt z, int n, SafeLong *seed)
1922
1923	{
1924
1925	int i;
1926	MrBFlt sum;
1927
1928	sum = 0.0;
1929	for(i=0; i<n; i++)
1930	{
1931	z[i] = RndGamma (alp[i], seed) / 1.0;
1932	sum += z[i];
1933	}
1934	for(i=0; i<n; i++)
1935	z[i] /= sum;
1936	}
1937
1938
1939
1940
1941
1942	/*---------------------------------------------------------------------------------
1943	\|
1944	\| DiscreteGamma
1945	\|
1946	\| Discretization of gamma distribution with equal proportions in each
1947	\| category.
1948	\|
1949	---------------------------------------------------------------------------------*/
1950	int DiscreteGamma (MrBFlt *rK, MrBFlt alfa, MrBFlt beta, int K, int median)
1951
1952	{
1953
1954	int i;
1955	MrBFlt gap05 = 1.0/(2.0K), t, factor = alfa/betaK, lnga1;
1956
1957	if (median)
1958	{
1959	for (i=0; i<K; i++)
1960	rK[i] = POINTGAMMA((i2.0+1.0)gap05, alfa, beta);
1961	for (i=0,t=0; i<K; i++)
1962	t += rK[i];
1963	for (i=0; i<K; i++)
1964	rK[i] *= factor / t;
1965	}
1966	else
1967	{
1968	lnga1 = LnGamma(alfa+1);
1969	/* calculate the points in the gamma distribution */
1970	for (i=0; i<K-1; i++)
1971	rK[i] = POINTGAMMA((i+1.0)/K, alfa, beta);
1972	/* calculate the cumulative values */
1973	for (i=0; i<K-1; i++)
1974	rK[i] = IncompleteGamma(rK[i] * beta, alfa + 1.0, lnga1);
1975	rK[K-1] = 1.0;
1976	/* calculate the relative values and rescale */
1977	for (i=K-1; i>0; i--)
1978	{
1979	rK[i] -= rK[i-1];
1980	rK[i] *= factor;
1981	}
1982	rK[0] *= factor;
1983	}
1984
1985	return (NO_ERROR);
1986
1987	}
1988
1989
1990
1991
1992
1993	/*---------------------------------------------------------------------------------
1994	\|
1995	\| DivideByTwos
1996	\|
1997	\| Divides all of the elements of the matrix a by 2^power.
1998	\|
1999	---------------------------------------------------------------------------------*/
2000	void DivideByTwos (int dim, MrBFlt **a, int power)
2001
2002	{
2003
2004	int divisor = 1, i, row, col;
2005
2006	for (i=0; i<power; i++)
2007	divisor = divisor * 2;
2008
2009	for (row=0; row<dim; row++)
2010	for (col=0; col<dim; col++)
2011	a[row][col] /= divisor;
2012
2013	}
2014
2015
2016
2017
2018
2019	/*---------------------------------------------------------------------------------
2020	\|
2021	\| D_sign
2022	\|
2023	\| This function is called from "Hqr2".
2024	\|
2025	---------------------------------------------------------------------------------*/
2026	MrBFlt D_sign (MrBFlt a, MrBFlt b)
2027
2028	{
2029
2030	MrBFlt x;
2031
2032	x = (a >= 0 ? a : -a);
2033
2034	return (b >= 0 ? x : -x);
2035
2036	}
2037
2038
2039
2040
2041
2042	/*---------------------------------------------------------------------------------
2043	\|
2044	\| Eigens
2045	\|
2046	\| The matrix of interest is a. The ouptut is the real and imaginary parts of the
2047	\| eigenvalues (wr and wi). z contains the real and imaginary parts of the
2048	\| eigenvectors. iv2 and fv1 are working vectors.
2049	\|
2050	---------------------------------------------------------------------------------*/
2051	int EigensForRealMatrix (int dim, MrBFlt *a, MrBFlt wr, MrBFlt wi, MrBFlt z, int iv1, MrBFlt *fv1)
2052
2053	{
2054
2055	static int is1, is2;
2056	int ierr;
2057
2058	Balanc (dim, a, &is1, &is2, fv1);
2059	ElmHes (dim, is1, is2, a, iv1);
2060	ElTran (dim, is1, is2, a, iv1, z);
2061	ierr = Hqr2 (dim, is1, is2, a, wr, wi, z);
2062	if (ierr == 0)
2063	BalBak (dim, is1, is2, fv1, dim, z);
2064
2065	return (ierr);
2066
2067	}
2068
2069
2070
2071
2072
2073	/*---------------------------------------------------------------------------------
2074	\|
2075	\| ElmHes
2076	\|
2077	\| Given a real general matrix, this subroutine
2078	\| reduces a submatrix situated in rows and columns
2079	\| low through high to upper Hessenberg form by
2080	\| stabilized elementary similarity transformations.
2081	\|
2082	\| On input:
2083	\|
2084	\| * dim is the order of the matrix
2085	\|
2086	\| * low and high are integers determined by the balancing
2087	\| subroutine balanc. if balanc has not been used,
2088	\| set low=1, high=dim.
2089	\|
2090	\| * a contains the input matrix.
2091	\|
2092	\| On output:
2093	\|
2094	\| * a contains the hessenberg matrix. The multipliers
2095	\| which were used in the reduction are stored in the
2096	\| remaining triangle under the hessenberg matrix.
2097	\|
2098	\| * interchanged contains information on the rows and columns
2099	\| interchanged in the reduction.
2100	\|
2101	\| Only elements low through high are used.
2102	\|
2103	---------------------------------------------------------------------------------*/
2104	void ElmHes (int dim, int low, int high, MrBFlt *a, int interchanged)
2105
2106	{
2107	int i, j, m, la, mm1, kp1, mp1;
2108	MrBFlt x, y;
2109
2110	la = high - 1;
2111	kp1 = low + 1;
2112	if (la < kp1)
2113	return; /* remove goto statement, which exits at bottom of function */
2114
2115	for (m=kp1; m<=la; m++)
2116	{
2117	mm1 = m - 1;
2118	x = 0.0;
2119	i = m;
2120
2121	for (j=m; j<=high; j++)
2122	{
2123	if (fabs(a[j][mm1]) > fabs(x)) /* change direction of inequality */
2124	{ /* remove goto statement */
2125	x = a[j][mm1];
2126	i = j;
2127	}
2128	}
2129
2130	interchanged[m] = i;
2131	if (i != m) /* change "==" to "!=", eliminating goto statement */
2132	{
2133	/* interchange rows and columns of a */
2134	for (j=mm1; j<dim; j++)
2135	{
2136	y = a[i][j];
2137	a[i][j] = a[m][j];
2138	a[m][j] = y;
2139	}
2140	for (j=0; j<=high; j++)
2141	{
2142	y = a[j][i];
2143	a[j][i] = a[j][m];
2144	a[j][m] = y;
2145	}
2146	}
2147
2148	if (AreDoublesEqual(x, 0.0, ETA)==NO) /* change "==" to "!=", eliminating goto statement */
2149	{
2150	mp1 = m + 1;
2151
2152	for (i=mp1; i<=high; i++)
2153	{
2154	y = a[i][mm1];
2155	if (AreDoublesEqual(y, 0.0, ETA)==NO) /* != 0.0 */
2156	{
2157	y /= x;
2158	a[i][mm1] = y;
2159	for (j = m; j < dim; j++)
2160	a[i][j] -= y * a[m][j];
2161	for (j = 0; j <= high; j++)
2162	a[j][m] += y * a[j][i];
2163	}
2164	}
2165	}
2166	}
2167
2168	#if 0
2169	/* begin f2c version of code:
2170	elmhes.f -- translated by f2c (version 19971204) */
2171	int elmhes (int nm, int n, int low, int igh, MrBFlt a, int int__)
2172
2173	{
2174
2175	/system generated locals /
2176	int a_dim1, a_offset, i__1, i__2, i__3;
2177	MrBFlt d__1;
2178
2179	/* local variables */
2180	static int i__, j, m;
2181	static MrBFlt x, y;
2182	static int la, mm1, kp1, mp1;
2183
2184	/* parameter adjustments */
2185	a_dim1 = *nm;
2186	a_offset = a_dim1 + 1;
2187	a -= a_offset;
2188	--int__;
2189
2190	/* function body */
2191	la = *igh - 1;
2192	kp1 = *low + 1;
2193	if (la < kp1)
2194	goto L200;
2195
2196	i__1 = la;
2197	for (m = kp1; m <= i__1; ++m)
2198	{
2199	mm1 = m - 1;
2200	x = 0.;
2201	i__ = m;
2202	i__2 = *igh;
2203	for (j = m; j <= i__2; ++j)
2204	{
2205	if ((d__1 = a[j + mm1 * a_dim1], abs(d__1)) <= abs(x))
2206	goto L100;
2207	x = a[j + mm1 * a_dim1];
2208	i__ = j;
2209	L100:
2210	;
2211	}
2212
2213	int__[m] = i__;
2214	if (i__ == m)
2215	goto L130;
2216
2217	/* .......... interchange rows and columns of a.......... */
2218	i__2 = *n;
2219	for (j = mm1; j <= i__2; ++j)
2220	{
2221	y = a[i__ + j * a_dim1];
2222	a[i__ + j * a_dim1] = a[m + j * a_dim1];
2223	a[m + j * a_dim1] = y;
2224	/* L110: */
2225	}
2226
2227	i__2 = *igh;
2228	for (j = 1; j <= i__2; ++j)
2229	{
2230	y = a[j + i__ * a_dim1];
2231	a[j + i__ * a_dim1] = a[j + m * a_dim1];
2232	a[j + m * a_dim1] = y;
2233	/* L120: */
2234	}
2235
2236	/* .......... end interchange .......... */
2237	L130:
2238	if (x == 0.)
2239	goto L180;
2240	mp1 = m + 1;
2241
2242	i__2 = *igh;
2243	for (i__ = mp1; i__ <= i__2; ++i__)
2244	{
2245	y = a[i__ + mm1 * a_dim1];
2246	if (y == 0.)
2247	goto L160;
2248	y /= x;
2249	a[i__ + mm1 * a_dim1] = y;
2250
2251	i__3 = *n;
2252	for (j = m; j <= i__3; ++j)
2253	{
2254	/* L140: */
2255	a[i__ + j * a_dim1] -= y * a[m + j * a_dim1];
2256	}
2257
2258	i__3 = *igh;
2259	for (j = 1; j <= i__3; ++j)
2260	{
2261	/* L150: */
2262	a[j + m * a_dim1] += y * a[j + i__ * a_dim1];
2263	}
2264
2265	L160:
2266	;
2267	}
2268
2269	L180:
2270	;
2271	}
2272
2273	L200:
2274	return 0;
2275
2276	}
2277	/* end f2c version of code */
2278	#endif
2279
2280	}
2281
2282
2283
2284
2285
2286	/*---------------------------------------------------------------------------------
2287	\|
2288	\| ElTran
2289	\|
2290	\| This subroutine accumulates the stabilized elementary
2291	\| similarity transformations used in the reduction of a
2292	\| real general matrix to upper Hessenberg form by ElmHes.
2293	\|
2294	\| On input:
2295	\|
2296	\| * dim is the order of the matrix.
2297	\|
2298	\| * low and high are integers determined by the balancing
2299	\| subroutine balanc. If Balanc has not been used,
2300	\| set low=0, high=dim-1.
2301	\|
2302	\| * a contains the multipliers which were used in the
2303	\| reduction by ElmHes in its lower triangle
2304	\| below the subdiagonal.
2305	\|
2306	\| * interchanged contains information on the rows and columns
2307	\| interchanged in the reduction by ElmHes.
2308	\| only elements low through high are used.
2309	\|
2310	\| On output:
2311	\|
2312	\| * z contains the transformation matrix produced in the
2313	\| reduction by ElmHes.
2314	\|
2315	\| This routine is a translation of the Algol procedure from
2316	\| Handbook for Automatic Computation, vol. II, Linear Algebra,
2317	\| by Wilkinson and Reinsch, Springer-Verlag.
2318	\|
2319	---------------------------------------------------------------------------------*/
2320	void ElTran (int dim, int low, int high, MrBFlt *a, int interchanged, MrBFlt **z)
2321
2322	{
2323
2324	int i, j, mp;
2325
2326	/* initialize z to identity matrix */
2327	for (j=0; j<dim; j++)
2328	{
2329	for (i=0; i<dim; i++)
2330	z[i][j] = 0.0;
2331	z[j][j] = 1.0;
2332	}
2333	for (mp=high-1; mp>=low+1; mp--) /* there were a number of additional */
2334	{ /* variables (kl, la, m, mm, mp1) that */
2335	for (i=mp+1; i<=high; i++) /* have been eliminated here simply by */
2336	z[i][mp] = a[i][mp-1]; /* initializing variables appropriately */
2337	i = interchanged[mp]; /* in the loops */
2338	if (i != mp) /* change "==" to "!=" to eliminate a goto statement */
2339	{
2340	for (j=mp; j<=high; j++)
2341	{
2342	z[mp][j] = z[i][j];
2343	z[i][j] = 0.0;
2344	}
2345	z[i][mp] = 1.0;
2346	}
2347	}
2348
2349	#if 0
2350	/* begin f2c version of code:
2351	eltran.f -- translated by f2c (version 19971204) */
2352	int eltran (int nm, int n, int low, int igh, MrBFlt a, int int__, MrBFlt *z__)
2353
2354	{
2355
2356	/* system generated locals */
2357	int a_dim1, a_offset, z_dim1, z_offset, i__1, i__2;
2358
2359	/* local variables */
2360	static int i__, j, kl, mm, mp, mp1;
2361
2362	/* .......... initialize z to identity matrix .......... */
2363
2364	/* parameter adjustments */
2365	z_dim1 = *nm;
2366	z_offset = z_dim1 + 1;
2367	z__ -= z_offset;
2368	--int__;
2369	a_dim1 = *nm;
2370	a_offset = a_dim1 + 1;
2371	a -= a_offset;
2372
2373	/* function Body */
2374	i__1 = *n;
2375	for (j = 1; j <= i__1; ++j)
2376	{
2377	i__2 = *n;
2378	for (i__ = 1; i__ <= i__2; ++i__)
2379	{
2380	/* L60: */
2381	z__[i__ + j * z_dim1] = 0.0;
2382	}
2383	z__[j + j * z_dim1] = 1.0;
2384	/* L80: */
2385	}
2386
2387	kl = igh - low - 1;
2388	if (kl < 1)
2389	goto L200;
2390
2391	/* .......... for mp=igh-1 step -1 until low+1 do -- .......... */
2392	i__1 = kl;
2393	for (mm = 1; mm <= i__1; ++mm)
2394	{
2395	mp = *igh - mm;
2396	mp1 = mp + 1;
2397	i__2 = *igh;
2398	for (i__ = mp1; i__ <= i__2; ++i__)
2399	{
2400	/* L100: */
2401	z__[i__ + mp * z_dim1] = a[i__ + (mp - 1) * a_dim1];
2402	}
2403	i__ = int__[mp];
2404	if (i__ == mp)
2405	goto L140;
2406	i__2 = *igh;
2407	for (j = mp; j <= i__2; ++j)
2408	{
2409	z__[mp + j * z_dim1] = z__[i__ + j * z_dim1];
2410	z__[i__ + j * z_dim1] = 0.;
2411	/* L130: */
2412	}
2413	z__[i__ + mp * z_dim1] = 1.;
2414	L140:
2415	;
2416	}
2417
2418	L200:
2419	return 0;
2420
2421	}
2422	/* end f2c version of code */
2423	#endif
2424
2425	}
2426
2427
2428
2429
2430
2431	/*---------------------------------------------------------------------------------
2432	\|
2433	\| Exchange
2434	\|
2435	---------------------------------------------------------------------------------*/
2436	void Exchange (int j, int k, int l, int m, int n, MrBFlt *a, MrBFlt scale)
2437
2438	{
2439
2440	int i;
2441	MrBFlt f;
2442
2443	scale[m] = (MrBFlt)j;
2444	if (j != m)
2445	{
2446	for (i = 0; i <= l; i++)
2447	{
2448	f = a[i][j];
2449	a[i][j] = a[i][m];
2450	a[i][m] = f;
2451	}
2452	for (i = k; i < n; i++)
2453	{
2454	f = a[j][i];
2455	a[j][i] = a[m][i];
2456	a[m][i] = f;
2457	}
2458	}
2459
2460	}
2461
2462
2463
2464
2465
2466	/*---------------------------------------------------------------------------------
2467	\|
2468	\| Factorial
2469	\|
2470	\| Returns x!
2471	\|
2472	---------------------------------------------------------------------------------*/
2473	MrBFlt Factorial (int x)
2474
2475	{
2476
2477	int i;
2478	MrBFlt fac;
2479
2480	fac = 1.0;
2481	for (i=0; i<x; i++)
2482	{
2483	fac *= (i+1);
2484	}
2485
2486	return (fac);
2487
2488	}
2489
2490
2491
2492
2493
2494	/*---------------------------------------------------------------------------------
2495	\|
2496	\| ForwardSubstitutionRow
2497	\|
2498	---------------------------------------------------------------------------------*/
2499	void ForwardSubstitutionRow (int dim, MrBFlt *L, MrBFlt b)
2500
2501	{
2502
2503	int i, j;
2504	MrBFlt dotProduct;
2505
2506	b[0] = b[0] / L[0][0];
2507	for (i=1; i<dim; i++)
2508	{
2509	dotProduct = 0.0;
2510	for (j=0; j<i; j++)
2511	dotProduct += L[i][j] * b[j];
2512	b[i] = (b[i] - dotProduct) / L[i][i];
2513	}
2514
2515	}
2516
2517
2518
2519
2520
2521	/*---------------------------------------------------------------------------------
2522	\|
2523	\| FreeSquareComplexMatrix
2524	\|
2525	\| Frees a matrix of complex numbers.
2526	\|
2527	---------------------------------------------------------------------------------*/
2528	void FreeSquareComplexMatrix (complex **m)
2529
2530	{
2531
2532	free((char *) (m[0]));
2533	free((char *) (m));
2534
2535	}
2536
2537
2538
2539
2540
2541	/*---------------------------------------------------------------------------------
2542	\|
2543	\| FreeSquareDoubleMatrix
2544	\|
2545	\| Frees a matrix of doubles.
2546	\|
2547	---------------------------------------------------------------------------------*/
2548	void FreeSquareDoubleMatrix (MrBFlt **m)
2549
2550	{
2551
2552	free((char *) (m[0]));
2553	free((char *) (m));
2554
2555	}
2556
2557
2558
2559
2560	/*---------------------------------------------------------------------------------
2561	\|
2562	\| FreeSquareIntegerMatrix
2563	\|
2564	\| Frees a matrix of integers.
2565	\|
2566	---------------------------------------------------------------------------------*/
2567	void FreeSquareIntegerMatrix (int **m)
2568
2569	{
2570
2571	free((char *) (m[0]));
2572	free((char *) (m));
2573
2574	}
2575
2576
2577
2578
2579
2580	/*---------------------------------------------------------------------------------
2581	\|
2582	\| GammaRandomVariable
2583	\|
2584	\| This function generates a gamma-distributed random variable with parameters
2585	\| a and b. The mean is E(X) = a / b and the variance is Var(X) = a / b^2.
2586	\|
2587	---------------------------------------------------------------------------------*/
2588	MrBFlt GammaRandomVariable (MrBFlt a, MrBFlt b, SafeLong *seed)
2589
2590	{
2591
2592	return (RndGamma (a, seed) / b);
2593
2594	}
2595
2596
2597
2598
2599
2600	/*---------------------------------------------------------------------------------
2601	\|
2602	\| GaussianElimination
2603	\|
2604	---------------------------------------------------------------------------------*/
2605	void GaussianElimination (int dim, MrBFlt a, MrBFlt bMat, MrBFlt **xMat)
2606
2607	{
2608
2609	int i, k;
2610	MrBFlt bVec, lMat, *uMat;
2611
2612	lMat = AllocateSquareDoubleMatrix (dim);
2613	uMat = AllocateSquareDoubleMatrix (dim);
2614	bVec = (MrBFlt )SafeMalloc((size_t) ((dim) sizeof(MrBFlt)));
2615	if (!bVec)
2616	{
2617	MrBayesPrint ("%s Error: Problem allocating bVec\n", spacer);
2618	exit (0);
2619	}
2620
2621	ComputeLandU (dim, a, lMat, uMat);
2622
2623	for (k=0; k<dim; k++)
2624	{
2625
2626	for (i=0; i<dim; i++)
2627	bVec[i] = bMat[i][k];
2628
2629	/* Answer of Ly = b (which is solving for y) is copied into b. */
2630	ForwardSubstitutionRow (dim, lMat, bVec);
2631
2632	/* Answer of Ux = y (solving for x and the y was copied into b above)
2633	is also copied into b. */
2634	BackSubstitutionRow (dim, uMat, bVec);
2635
2636	for (i=0; i<dim; i++)
2637	xMat[i][k] = bVec[i];
2638
2639	}
2640
2641	FreeSquareDoubleMatrix (lMat);
2642	FreeSquareDoubleMatrix (uMat);
2643	free (bVec);
2644
2645	}
2646
2647
2648
2649
2650
2651	/*---------------------------------------------------------------------------------
2652	\|
2653	\| GetEigens
2654	\|
2655	\| returns NO if non complex eigendecomposition, YES if complex eigendecomposition, ABORT if an error has occured
2656	\|
2657	---------------------------------------------------------------------------------*/
2658	int GetEigens (int dim, MrBFlt *q, MrBFlt eigenValues, MrBFlt eigvalsImag, MrBFlt eigvecs, MrBFlt inverseEigvecs, complex Ceigvecs, complex *CinverseEigvecs)
2659
2660	{
2661
2662	int i, j, rc, *iWork, isComplex;
2663	MrBFlt *tempWork, dWork;
2664	complex *cWork, Ccol;
2665
2666	/* allocate memory */
2667	dWork = (MrBFlt )SafeMalloc((size_t) (dim sizeof(MrBFlt)));
2668	iWork = (int )SafeMalloc((size_t) (dim sizeof(int)));
2669	if (!dWork \|\| !iWork)
2670	{
2671	MrBayesPrint ("%s Error: Problem in GetEigens\n", spacer);
2672	exit (0);
2673	}
2674
2675	/* calculate eigenvalues and eigenvectors */
2676	isComplex = NO;
2677	rc = ComputeEigenSystem (dim, q, eigenValues, eigvalsImag, eigvecs, iWork, dWork);
2678	if (rc != NO_ERROR)
2679	{
2680	if (rc == EVALUATE_COMPLEX_NUMBERS)
2681	isComplex = YES;
2682	else
2683	isComplex = ABORT;
2684	}
2685
2686	/* invert eigenvectors */
2687	if (isComplex == NO)
2688	{
2689	tempWork = AllocateSquareDoubleMatrix (dim);
2690	CopyDoubleMatrices (dim, eigvecs, tempWork);
2691	InvertMatrix (dim, tempWork, dWork, iWork, inverseEigvecs);
2692	FreeSquareDoubleMatrix (tempWork);
2693	}
2694	else if (isComplex == YES)
2695	{
2696	for(i=0; i<dim; i++)
2697	{
2698	if (fabs(eigvalsImag[i])<1E-20) /* == 0.0 */
2699	{
2700	for(j=0; j<dim; j++)
2701	{
2702	Ceigvecs[j][i].re = eigvecs[j][i];
2703	Ceigvecs[j][i].im = 0.0;
2704	}
2705	}
2706	else if (eigvalsImag[i] > 0)
2707	{
2708	for (j=0; j<dim; j++)
2709	{
2710	Ceigvecs[j][i].re = eigvecs[j][i];
2711	Ceigvecs[j][i].im = eigvecs[j][i + 1];
2712	}
2713	}
2714	else if (eigvalsImag[i] < 0)
2715	{
2716	for (j=0; j<dim; j++)
2717	{
2718	Ceigvecs[j][i].re = eigvecs[j][i-1];
2719	Ceigvecs[j][i].im = -eigvecs[j][i];
2720	}
2721	}
2722	}
2723	Ccol = (complex )SafeMalloc((size_t) (dim sizeof(complex)));
2724	if (!Ccol)
2725	{
2726	MrBayesPrint ("%s Error: Problem in GetEigens\n", spacer);
2727	exit (0);
2728	}
2729	cWork = AllocateSquareComplexMatrix (dim);
2730	CopyComplexMatrices (dim, Ceigvecs, cWork);
2731	ComplexInvertMatrix (dim, cWork, dWork, iWork, CinverseEigvecs, Ccol);
2732	free (Ccol);
2733	FreeSquareComplexMatrix (cWork);
2734	}
2735
2736	free (dWork);
2737	free (iWork);
2738
2739	return (isComplex);
2740
2741	}
2742
2743
2744
2745
2746
2747	/*---------------------------------------------------------------------------------
2748	\|
2749	\| Hqr2
2750	\|
2751	\| This subroutine finds the eigenvalues and eigenvectors
2752	\| of a real upper Hessenberg matrix by the QR method. The
2753	\| eigenvectors of a real general matrix can also be found
2754	\| if ElmHes and ElTran or OrtHes and OrTran have
2755	\| been used to reduce this general matrix to Hessenberg form
2756	\| and to accumulate the similarity transformations.
2757	\|
2758	\| On input:
2759	\|
2760	\| * dim is the order of the matrix.
2761	\|
2762	\| * low and high are integers determined by the balancing
2763	\| subroutine balanc. If balanc has not been used,
2764	\| set low=0, high=dim-1.
2765	\|
2766	\| * h contains the upper hessenberg matrix. Information about
2767	\| the transformations used in the reduction to Hessenberg
2768	\| form by ElmHes or OrtHes, if performed, is stored
2769	\| in the remaining triangle under the Hessenberg matrix.
2770	\|
2771	\| On output:
2772	\|
2773	\| * h has been destroyed.
2774	\|
2775	\| * wr and wi contain the real and imaginary parts,
2776	\| respectively, of the eigenvalues. The eigenvalues
2777	\| are unordered except that complex conjugate pairs
2778	\| of values appear consecutively with the eigenvalue
2779	\| having the positive imaginary part first. If an
2780	\| error exit is made, the eigenvalues should be correct
2781	\| for indices j,...,dim-1.
2782	\|
2783	\| * z contains the transformation matrix produced by ElTran
2784	\| after the reduction by ElmHes, or by OrTran after the
2785	\| reduction by OrtHes, if performed. If the eigenvectors
2786	\| of the Hessenberg matrix are desired, z must contain the
2787	\| identity matrix.
2788	\|
2789	\| Calls ComplexDivision2 for complex division.
2790	\|
2791	\| This function returns:
2792	\| zero for normal return,
2793	\| j if the limit of 30*n iterations is exhausted
2794	\| while the j-th eigenvalue is being sought.
2795	\|
2796	\| This subroutine is a translation of the ALGOL procedure HQR2,
2797	\| Num. Math. 14, 219,231(1970) by Martin, Peters, and Wilkinson.
2798	\| Handbook for Automatic Computation, vol. II - Linear Algebra,
2799	\| pp. 357-391 (1971).
2800	\|
2801	---------------------------------------------------------------------------------*/
2802	int Hqr2 (int dim, int low, int high, MrBFlt *h, MrBFlt wr, MrBFlt wi, MrBFlt *z)
2803
2804	{
2805
2806	int i, j, k, l, m, na, en, notlas, mp2, itn, its, enm2, twoRoots;
2807	MrBFlt norm, p=0.0, q=0.0, r=0.0, s=0.0, t, w=0.0, x, y=0.0, ra, sa, vi, vr, zz=0.0, tst1, tst2;
2808
2809	norm = 0.0;
2810	k = 0; /* used for array indexing. FORTRAN version: k = 1 */
2811
2812	/* store roots isolated by balance, and compute matrix norm */
2813	for (i=0; i<dim; i++)
2814	{
2815	for (j=k; j<dim; j++)
2816	norm += fabs(h[i][j]);
2817
2818	k = i;
2819	if ((i < low) \|\| (i > high))
2820	{
2821	wr[i] = h[i][i];
2822	wi[i] = 0.0;
2823	}
2824	}
2825	en = high;
2826	t = 0.0;
2827	itn = dim * 30;
2828
2829	/* search for next eigenvalues */
2830	while (en >= low) /* changed from an "if(en < lo)" to eliminate a goto statement */
2831	{
2832	its = 0;
2833	na = en - 1;
2834	enm2 = na - 1;
2835	twoRoots = FALSE;
2836
2837	for (;;)
2838	{
2839	for (l=en; l>low; l--) /* changed indexing, got rid of lo, ll */
2840	{
2841	s = fabs(h[l-1][l-1]) + fabs(h[l][l]);
2842	if (AreDoublesEqual(s, 0.0, ETA)==YES) /* == 0.0 */
2843	s = norm;
2844	tst1 = s;
2845	tst2 = tst1 + fabs(h[l][l-1]);
2846	if (fabs(tst2 - tst1) < ETA) /* tst2 == tst1 */
2847	break; /* changed to break to remove a goto statement */
2848	}
2849
2850	/* form shift */
2851	x = h[en][en];
2852	if (l == en) /* changed to break to remove a goto statement */
2853	break;
2854	y = h[na][na];
2855	w = h[en][na] * h[na][en];
2856	if (l == na) /* used to return to other parts of the code */
2857	{
2858	twoRoots = TRUE;
2859	break;
2860	}
2861	if (itn == 0)
2862	return (en);
2863
2864	/* form exceptional shift */
2865	if ((its == 10) \|\| (its == 20)) /* changed to remove a goto statement */
2866	{
2867	t += x;
2868	for (i = low; i <= en; i++)
2869	h[i][i] -= x;
2870	s = fabs(h[en][na]) + fabs(h[na][enm2]);
2871	x = 0.75 * s;
2872	y = x;
2873	w = -0.4375 * s * s;
2874	}
2875	its++;
2876	itn--;
2877
2878	/* look for two consecutive small sub-diagonal elements */
2879	for (m=enm2; m>=l; m--)
2880	{
2881	/* removed m = enm2 + l - mm and above loop to remove variables */
2882	zz = h[m][m];
2883	r = x - zz;
2884	s = y - zz;
2885	p = (r * s - w) / h[m+1][m] + h[m][m+1];
2886	q = h[m+1][m+1] - zz - r - s;
2887	r = h[m+2][m+1];
2888	s = fabs(p) + fabs(q) + fabs(r);
2889	p /= s;
2890	q /= s;
2891	r /= s;
2892	if (m == l)
2893	break; /* changed to break to remove a goto statement */
2894	tst1 = fabs(p) * (fabs(h[m-1][m-1]) + fabs(zz) + fabs(h[m+1][m+1]));
2895	tst2 = tst1 + fabs(h[m][m-1]) * (fabs(q) + fabs(r));
2896	if (fabs(tst2 - tst1) < ETA) /* tst2 == tst1 */
2897	break; /* changed to break to remove a goto statement */
2898	}
2899
2900	mp2 = m + 2;
2901	for (i = mp2; i <= en; i++)
2902	{
2903	h[i][i-2] = 0.0;
2904	if (i != mp2) /* changed "==" to "!=" to remove a goto statement */
2905	h[i][i-3] = 0.0;
2906	}
2907
2908	/* MrBFlt QR step involving rows l to en and columns m to en */
2909	for (k=m; k<=na; k++)
2910	{
2911	notlas = (k != na);
2912	if (k != m) /* changed "==" to "!=" to remove a goto statement */
2913	{
2914	p = h[k][k-1];
2915	q = h[k+1][k-1];
2916	r = 0.0;
2917	if (notlas)
2918	r = h[k+2][k-1];
2919	x = fabs(p) + fabs(q) + fabs(r);
2920	if (x < ETA) /* == 0.0 */
2921	continue; /* changed to continue remove a goto statement */
2922	p /= x;
2923	q /= x;
2924	r /= x;
2925	}
2926
2927	/s = sqrt(pp+qq+rr);
2928	sgn = (p<0)?-1:(p>0);
2929	s = sgnsqrt(pp+qq+rr);*/
2930	s = D_sign(sqrt(pp + qq + r*r), p);
2931	if (k != m) /* changed "==" to "!=" to remove a goto statement */
2932	h[k][k-1] = -s * x;
2933	else if (l != m) /* else if gets rid of another goto statement */
2934	h[k][k-1] = -h[k][k-1];
2935	p += s;
2936	x = p / s;
2937	y = q / s;
2938	zz = r / s;
2939	q /= p;
2940	r /= p;
2941	if (!notlas) /* changed to !notlas to remove goto statement (see *) /
2942	{
2943	/* row modification */
2944	for (j=k; j<dim; j++)
2945	{
2946	p = h[k][j] + q * h[k+1][j];
2947	h[k][j] -= p * x;
2948	h[k+1][j] -= p * y;
2949	}
2950	j = MIN(en, k + 3);
2951
2952	/* column modification */
2953	for (i=0; i<=j; i++)
2954	{
2955	p = x * h[i][k] + y * h[i][k+1];
2956	h[i][k] -= p;
2957	h[i][k+1] -= p * q;
2958	}
2959
2960	/* accumulate transformations */
2961	for (i=low; i<=high; i++)
2962	{
2963	p = x * z[i][k] + y * z[i][k+1];
2964	z[i][k] -= p;
2965	z[i][k+1] -= p * q;
2966	}
2967	}
2968	else /* (*) also put in else /
2969	{
2970	/* row modification */
2971	for (j=k; j<dim; j++)
2972	{
2973	p = h[k][j] + q * h[k+1][j] + r * h[k+2][j];
2974	h[k][j] -= p * x;
2975	h[k+1][j] -= p * y;
2976	h[k+2][j] -= p * zz;
2977	}
2978	j = MIN(en, k + 3);
2979
2980	/* column modification */
2981	for (i = 0; i <= j; i++)
2982	{
2983	p = x * h[i][k] + y * h[i][k+1] + zz * h[i][k+2];
2984	h[i][k] -= p;
2985	h[i][k+1] -= p * q;
2986	h[i][k+2] -= p * r;
2987	}
2988
2989	/* accumulate transformations */
2990	for (i = low; i <= high; i++)
2991	{
2992	p = x * z[i][k] + y * z[i][k+1] + zz * z[i][k+2];
2993	z[i][k] -= p;
2994	z[i][k+1] -= p * q;
2995	z[i][k+2] -= p * r;
2996	}
2997	}
2998	}
2999	}
3000
3001	if (twoRoots)
3002	{
3003	/* two roots found */
3004	p = (y - x) / 2.0;
3005	q = p * p + w;
3006	zz = sqrt(fabs(q));
3007	h[en][en] = x + t;
3008	x = h[en][en];
3009	h[na][na] = y + t;
3010	if (q >= -1e-12) /* change "<" to ">=", and also change "0.0" to */
3011	{ /* a small number (Swofford's change) */
3012	/* real pair */
3013	zz = p + D_sign(zz, p);
3014	wr[na] = x + zz;
3015	wr[en] = wr[na];
3016	if (fabs(zz) > ETA) /* != 0.0 */
3017	wr[en] = x - w/zz;
3018	wi[na] = 0.0;
3019	wi[en] = 0.0;
3020	x = h[en][na];
3021	s = fabs(x) + fabs(zz);
3022	p = x / s;
3023	q = zz / s;
3024	r = sqrt(pp + qq);
3025	p /= r;
3026	q /= r;
3027
3028	/* row modification */
3029	for (j=na; j<dim; j++)
3030	{
3031	zz = h[na][j];
3032	h[na][j] = q * zz + p * h[en][j];
3033	h[en][j] = q * h[en][j] - p * zz;
3034	}
3035
3036	/* column modification */
3037	for (i = 0; i <= en; i++)
3038	{
3039	zz = h[i][na];
3040	h[i][na] = q * zz + p * h[i][en];
3041	h[i][en] = q * h[i][en] - p * zz;
3042	}
3043
3044	/* accumulate transformations */
3045	for (i = low; i <= high; i++)
3046	{
3047	zz = z[i][na];
3048	z[i][na] = q * zz + p * z[i][en];
3049	z[i][en] = q * z[i][en] - p * zz;
3050	}
3051	}
3052	else
3053	{
3054	/* complex pair */
3055	wr[na] = x + p;
3056	wr[en] = x + p;
3057	wi[na] = zz;
3058	wi[en] = -zz;
3059	}
3060	en = enm2;
3061	}
3062	else
3063	{
3064	/* one root found */
3065	h[en][en] = x + t;
3066	wr[en] = h[en][en];
3067	wi[en] = 0.0;
3068	en = na;
3069	}
3070	}
3071
3072	if (fabs(norm) < ETA) /* == 0.0 */
3073	return (0); /* was a goto end of function */
3074
3075	for (en=dim-1; en>=0; en--)
3076	{
3077	/en = n - nn - 1; and change for loop /
3078	p = wr[en];
3079	q = wi[en];
3080	na = en - 1;
3081
3082	if (q < -1e-12)
3083	{
3084	/* last vector component chosen imaginary so that eigenvector
3085	matrix is triangular */
3086	m = na;
3087	if (fabs(h[en][na]) > fabs(h[na][en]))
3088	{
3089	h[na][na] = q / h[en][na];
3090	h[na][en] = -(h[en][en] - p) / h[en][na];
3091	}
3092	else
3093	ComplexDivision2 (0.0, -h[na][en], h[na][na] - p, q, &h[na][na], &h[na][en]);
3094
3095	h[en][na] = 0.0;
3096	h[en][en] = 1.0;
3097	enm2 = na - 1;
3098	if (enm2 >= 0) /* changed direction to remove goto statement */
3099	{
3100	for (i=enm2; i>=0; i--)
3101	{
3102	w = h[i][i] - p;
3103	ra = 0.0;
3104	sa = 0.0;
3105
3106	for (j=m; j<=en; j++)
3107	{
3108	ra += h[i][j] * h[j][na];
3109	sa += h[i][j] * h[j][en];
3110	}
3111
3112	if (wi[i] < 0.0) /* changed direction to remove goto statement */
3113	{
3114	zz = w;
3115	r = ra;
3116	s = sa;
3117	}
3118	else
3119	{
3120	m = i;
3121	if (fabs(wi[i])<ETA) /* == 0.0 / / changed direction to remove goto statement */
3122	ComplexDivision2 (-ra, -sa, w, q, &h[i][na], &h[i][en]);
3123	else
3124	{
3125	/* solve complex equations */
3126	x = h[i][i+1];
3127	y = h[i+1][i];
3128	vr = (wr[i] - p) * (wr[i] - p) + wi[i] * wi[i] - q * q;
3129	vi = (wr[i] - p) * 2.0 * q;
3130	if ((fabs(vr)<ETA) && (fabs(vi)<ETA))
3131	{
3132	tst1 = norm * (fabs(w) + fabs(q) + fabs(x) + fabs(y) + fabs(zz));
3133	vr = tst1;
3134	do {
3135	vr *= .01;
3136	tst2 = tst1 + vr;
3137	}
3138	while (tst2 > tst1); /* made into a do/while loop */
3139	}
3140	ComplexDivision2 (x * r - zz * ra + q * sa, x * s - zz * sa - q * ra, vr, vi, &h[i][na], &h[i][en]);
3141	if (fabs(x) > fabs(zz) + fabs(q)) /* changed direction to remove goto statement */
3142	{
3143	h[i+1][na] = (-ra - w * h[i][na] + q * h[i][en]) / x;
3144	h[i+1][en] = (-sa - w * h[i][en] - q * h[i][na]) / x;
3145	}
3146	else
3147	ComplexDivision2 (-r - y * h[i][na], -s - y * h[i][en], zz, q, &h[i+1][na], &h[i+1][en]);
3148	}
3149
3150	/* overflow control */
3151	tst1 = fabs(h[i][na]);
3152	tst2 = fabs(h[i][en]);
3153	t = MAX(tst1, tst2);
3154	if (t > ETA) /* t != 0.0 */
3155	{
3156	tst1 = t;
3157	tst2 = tst1 + 1.0 / tst1;
3158	if (tst2 <= tst1)
3159	{
3160	for (j = i; j <= en; j++)
3161	{
3162	h[j][na] /= t;
3163	h[j][en] /= t;
3164	}
3165	}
3166	}
3167	}
3168	}
3169	}
3170	}
3171	else if (fabs(q)<ETA)
3172	{
3173	/* real vector */
3174	m = en;
3175	h[en][en] = 1.0;
3176	if (na >= 0)
3177	{
3178	for (i=na; i>=0; i--)
3179	{
3180	w = h[i][i] - p;
3181	r = 0.0;
3182	for (j = m; j <= en; j++)
3183	r += h[i][j] * h[j][en];
3184	if (wi[i] < 0.0) /* changed direction to remove goto statement */
3185	{
3186	zz = w;
3187	s = r;
3188	continue; /* changed to continue to remove goto statement */
3189	}
3190	else
3191	{
3192	m = i;
3193	if (fabs(wi[i])<ETA) /* changed to remove goto statement */
3194	{
3195	t = w;
3196	if (fabs(t)<ETA) /* changed to remove goto statement */
3197	{
3198	tst1 = norm;
3199	t = tst1;
3200	do {
3201	t *= .01;
3202	tst2 = norm + t;
3203	}
3204	while (tst2 > tst1);
3205	}
3206	h[i][en] = -r / t;
3207	}
3208	else
3209	{
3210	/* solve real equations */
3211	x = h[i][i+1];
3212	y = h[i+1][i];
3213	q = (wr[i] - p) * (wr[i] - p) + wi[i] * wi[i];
3214	t = (x * s - zz * r) / q;
3215	h[i][en] = t;
3216	if (fabs(x) > fabs(zz)) /* changed direction to remove goto statement */
3217	h[i+1][en] = (-r - w * t) / x;
3218	else
3219	h[i+1][en] = (-s - y * t) / zz;
3220	}
3221
3222	/* overflow control */
3223	t = fabs(h[i][en]);
3224	if (t > ETA)
3225	{
3226	tst1 = t;
3227	tst2 = tst1 + 1. / tst1;
3228	if (tst2 <= tst1)
3229	{
3230	for (j = i; j <= en; j++)
3231	h[j][en] /= t;
3232	}
3233	}
3234	}
3235	}
3236	}
3237	}
3238	}
3239
3240	for (i=0; i<dim; i++)
3241	{
3242	if ((i < low) \|\| (i > high)) /* changed to rid goto statement */
3243	{
3244	for (j=i; j<dim; j++)
3245	z[i][j] = h[i][j];
3246	}
3247	}
3248
3249	/* multiply by transformation matrix to give vectors of original
3250	full matrix */
3251	for (j=dim-1; j>=low; j--)
3252	{
3253	m = MIN(j, high);
3254	for (i=low; i<=high; i++)
3255	{
3256	zz = 0.0;
3257	for (k = low; k <= m; k++)
3258	zz += z[i][k] * h[k][j];
3259	z[i][j] = zz;
3260	}
3261	}
3262
3263	return (0);
3264
3265	#if 0
3266	int hqr2 (int nm, int n, int low, int igh, MrBFlt h__, MrBFlt wr, MrBFlt wi, MrBFlt z__, int *ierr)
3267
3268	{
3269
3270	/* system generated locals */
3271	int h_dim1, h_offset, z_dim1, z_offset, i__1, i__2, i__3;
3272	MrBFlt d__1, d__2, d__3, d__4;
3273
3274	/* builtin functions */
3275	MrBFlt sqrt(doublereal), d_sign(doublereal , doublereal );
3276
3277	/* Local variables */
3278	static MrBFlt norm;
3279	static int i__, j, k, l, m;
3280	static MrBFlt p, q, r__, s, t, w, x, y;
3281	static int na, ii, en, jj;
3282	static MrBFlt ra, sa;
3283	static int ll, mm, nn;
3284	static MrBFlt vi, vr, zz;
3285	static logical notlas;
3286	static int mp2, itn, its, enm2;
3287	static MrBFlt tst1, tst2;
3288
3289	/* parameter adjustments */
3290	z_dim1 = *nm;
3291	z_offset = z_dim1 + 1;
3292	z__ -= z_offset;
3293	--wi;
3294	--wr;
3295	h_dim1 = *nm;
3296	h_offset = h_dim1 + 1;
3297	h__ -= h_offset;
3298
3299	/* function Body */
3300	*ierr = 0;
3301	norm = 0.;
3302	k = 1;
3303
3304	/* .......... store roots isolated by balanc and compute matrix norm .......... */
3305	i__1 = *n;
3306	for (i__ = 1; i__ <= i__1; ++i__)
3307	{
3308	i__2 = *n;
3309	for (j = k; j <= i__2; ++j)
3310	{
3311	/* L40: */
3312	norm += (d__1 = h__[i__ + j * h_dim1], abs(d__1));
3313	}
3314	k = i__;
3315	if (i__ >= low && i__ <= igh)
3316	goto L50;
3317	wr[i__] = h__[i__ + i__ * h_dim1];
3318	wi[i__] = 0.;
3319	L50:
3320	;
3321	}
3322
3323	en = *igh;
3324	t = 0.;
3325	itn = n 30;
3326
3327	/* ..........search for next eigenvalues.......... */
3328	L60:
3329	if (en < *low)
3330	goto L340;
3331	its = 0;
3332	na = en - 1;
3333	enm2 = na - 1;
3334
3335	/* ..........look for single small sub-diagonal element for l=en step -1 until low do -- .......... */
3336	L70:
3337	i__1 = en;
3338	for (ll = *low; ll <= i__1; ++ll)
3339	{
3340	l = en + *low - ll;
3341	if (l == *low)
3342	goto L100;
3343	s = (d__1 = h__[l - 1 + (l - 1) * h_dim1], abs(d__1)) + (d__2 = h__[l + l * h_dim1], abs(d__2));
3344	if (s == 0.0)
3345	s = norm;
3346	tst1 = s;
3347	tst2 = tst1 + (d__1 = h__[l + (l - 1) * h_dim1], abs(d__1));
3348	if (tst2 == tst1)
3349	goto L100;
3350	/* L80: */
3351	}
3352
3353	/* .......... form shift .......... */
3354	L100:
3355	x = h__[en + en * h_dim1];
3356	if (l == en)
3357	goto L270;
3358	y = h__[na + na * h_dim1];
3359	w = h__[en + na * h_dim1] * h__[na + en * h_dim1];
3360	if (l == na)
3361	goto L280;
3362	if (itn == 0)
3363	goto L1000;
3364	if (its != 10 && its != 20)
3365	goto L130;
3366
3367	/* .......... form exceptional shift .......... */
3368	t += x;
3369
3370	i__1 = en;
3371	for (i__ = *low; i__ <= i__1; ++i__)
3372	{
3373	/* L120: */
3374	h__[i__ + i__ * h_dim1] -= x;
3375	}
3376
3377	s = (d__1 = h__[en + na * h_dim1], abs(d__1)) + (d__2 = h__[na + enm2 * h_dim1], abs(d__2));
3378	x = s * 0.75;
3379	y = x;
3380	w = s * -0.4375 * s;
3381	L130:
3382	++its;
3383	--itn;
3384
3385	/* .......... look for two consecutive small sub-diagonal elements for m=en-2 step -1 until l do -- .......... */
3386	i__1 = enm2;
3387	for (mm = l; mm <= i__1; ++mm)
3388	{
3389	m = enm2 + l - mm;
3390	zz = h__[m + m * h_dim1];
3391	r__ = x - zz;
3392	s = y - zz;
3393	p = (r__ * s - w) / h__[m + 1 + m * h_dim1] + h__[m + (m + 1) * h_dim1];
3394	q = h__[m + 1 + (m + 1) * h_dim1] - zz - r__ - s;
3395	r__ = h__[m + 2 + (m + 1) * h_dim1];
3396	s = abs(p) + abs(q) + abs(r__);
3397	p /= s;
3398	q /= s;
3399	r__ /= s;
3400	if (m == l)
3401	goto L150;
3402	tst1 = abs(p) * ((d__1 = h__[m - 1 + (m - 1) * h_dim1], abs(d__1)) +
3403	abs(zz) + (d__2 = h__[m + 1 + (m + 1) * h_dim1], abs(d__2)));
3404	tst2 = tst1 + (d__1 = h__[m + (m - 1) * h_dim1], abs(d__1)) * (abs(q) + abs(r__));
3405	if (tst2 == tst1)
3406	goto L150;
3407	/* L140: */
3408	}
3409	L150:
3410	mp2 = m + 2;
3411
3412	i__1 = en;
3413	for (i__ = mp2; i__ <= i__1; ++i__)
3414	{
3415	h__[i__ + (i__ - 2) * h_dim1] = 0.0;
3416	if (i__ == mp2)
3417	goto L160;
3418	h__[i__ + (i__ - 3) * h_dim1] = 0.;
3419	L160:
3420	;
3421	}
3422
3423	/* .......... MrBFlt qr step involving rows l to en and columns m to en .......... */
3424	i__1 = na;
3425	for (k = m; k <= i__1; ++k)
3426	{
3427	notlas = k != na;
3428	if (k == m)
3429	goto L170;
3430	p = h__[k + (k - 1) * h_dim1];
3431	q = h__[k + 1 + (k - 1) * h_dim1];
3432	r__ = 0.;
3433	if (notlas)
3434	r__ = h__[k + 2 + (k - 1) * h_dim1];
3435	x = abs(p) + abs(q) + abs(r__);
3436	if (x == 0.)
3437	goto L260;
3438	p /= x;
3439	q /= x;
3440	r__ /= x;
3441	L170:
3442	d__1 = sqrt(p * p + q * q + r__ * r__);
3443	s = d_sign(&d__1, &p);
3444	if (k == m)
3445	goto L180;
3446	h__[k + (k - 1) * h_dim1] = -s * x;
3447	goto L190;
3448	L180:
3449	if (l != m)
3450	{
3451	h__[k + (k - 1) * h_dim1] = -h__[k + (k - 1) * h_dim1];
3452	}
3453	L190:
3454	p += s;
3455	x = p / s;
3456	y = q / s;
3457	zz = r__ / s;
3458	q /= p;
3459	r__ /= p;
3460	if (notlas)
3461	goto L225;
3462
3463	/* .......... row modification .......... */
3464	i__2 = *n;
3465	for (j = k; j <= i__2; ++j)
3466	{
3467	p = h__[k + j * h_dim1] + q * h__[k + 1 + j * h_dim1];
3468	h__[k + j * h_dim1] -= p * x;
3469	h__[k + 1 + j * h_dim1] -= p * y;
3470	/* L200: */
3471	}
3472
3473	/* computing MIN */
3474	i__2 = en, i__3 = k + 3;
3475	j = min(i__2,i__3);
3476
3477	/* .......... column modification .......... */
3478	i__2 = j;
3479	for (i__ = 1; i__ <= i__2; ++i__)
3480	{
3481	p = x * h__[i__ + k * h_dim1] + y * h__[i__ + (k + 1) * h_dim1];
3482	h__[i__ + k * h_dim1] -= p;
3483	h__[i__ + (k + 1) * h_dim1] -= p * q;
3484	/* L210: */
3485	}
3486
3487	/* .......... accumulate transformations .......... */
3488	i__2 = *igh;
3489	for (i__ = *low; i__ <= i__2; ++i__)
3490	{
3491	p = x * z__[i__ + k * z_dim1] + y * z__[i__ + (k + 1) * z_dim1];
3492	z__[i__ + k * z_dim1] -= p;
3493	z__[i__ + (k + 1) * z_dim1] -= p * q;
3494	/* L220: */
3495	}
3496	goto L255;
3497	L225:
3498
3499	/* .......... row modification .......... */
3500	i__2 = *n;
3501	for (j = k; j <= i__2; ++j)
3502	{
3503	p = h__[k + j * h_dim1] + q * h__[k + 1 + j * h_dim1] + r__ * h__[k + 2 + j * h_dim1];
3504	h__[k + j * h_dim1] -= p * x;
3505	h__[k + 1 + j * h_dim1] -= p * y;
3506	h__[k + 2 + j * h_dim1] -= p * zz;
3507	/* L230: */
3508	}
3509
3510	/* computing MIN */
3511	i__2 = en, i__3 = k + 3;
3512	j = min(i__2,i__3);
3513
3514	/* .......... column modification .......... */
3515	i__2 = j;
3516	for (i__ = 1; i__ <= i__2; ++i__)
3517	{
3518	p = x * h__[i__ + k * h_dim1] + y * h__[i__ + (k + 1) * h_dim1] +
3519	zz * h__[i__ + (k + 2) * h_dim1];
3520	h__[i__ + k * h_dim1] -= p;
3521	h__[i__ + (k + 1) * h_dim1] -= p * q;
3522	h__[i__ + (k + 2) * h_dim1] -= p * r__;
3523	/* L240: */
3524	}
3525
3526	/* .......... accumulate transformations .......... */
3527	i__2 = *igh;
3528	for (i__ = *low; i__ <= i__2; ++i__)
3529	{
3530	p = x * z__[i__ + k * z_dim1] + y * z__[i__ + (k + 1) * z_dim1] + zz * z__[i__ + (k + 2) * z_dim1];
3531	z__[i__ + k * z_dim1] -= p;
3532	z__[i__ + (k + 1) * z_dim1] -= p * q;
3533	z__[i__ + (k + 2) * z_dim1] -= p * r__;
3534	/* L250: */
3535	}
3536	L255:
3537	L260:
3538	;
3539	}
3540	goto L70;
3541
3542	/* .......... one root found .......... */
3543	L270:
3544	h__[en + en * h_dim1] = x + t;
3545	wr[en] = h__[en + en * h_dim1];
3546	wi[en] = 0.;
3547	en = na;
3548	goto L60;
3549
3550	/* .......... two roots found .......... */
3551	L280:
3552	p = (y - x) / 2.;
3553	q = p * p + w;
3554	zz = sqrt((abs(q)));
3555	h__[en + en * h_dim1] = x + t;
3556	x = h__[en + en * h_dim1];
3557	h__[na + na * h_dim1] = y + t;
3558	if (q < 0.)
3559	goto L320;
3560
3561	/* .......... real pair .......... */
3562	zz = p + d_sign(&zz, &p);
3563	wr[na] = x + zz;
3564	wr[en] = wr[na];
3565	if (zz != 0.)
3566	{
3567	wr[en] = x - w / zz;
3568	}
3569	wi[na] = 0.0;
3570	wi[en] = 0.0;
3571	x = h__[en + na * h_dim1];
3572	s = abs(x) + abs(zz);
3573	p = x / s;
3574	q = zz / s;
3575	r__ = sqrt(p * p + q * q);
3576	p /= r__;
3577	q /= r__;
3578
3579	/* .......... row modification .......... */
3580	i__1 = *n;
3581	for (j = na; j <= i__1; ++j)
3582	{
3583	zz = h__[na + j * h_dim1];
3584	h__[na + j * h_dim1] = q * zz + p * h__[en + j * h_dim1];
3585	h__[en + j * h_dim1] = q * h__[en + j * h_dim1] - p * zz;
3586	/* L290: */
3587	}
3588
3589	/* .......... column modification .......... */
3590	i__1 = en;
3591	for (i__ = 1; i__ <= i__1; ++i__)
3592	{
3593	zz = h__[i__ + na * h_dim1];
3594	h__[i__ + na * h_dim1] = q * zz + p * h__[i__ + en * h_dim1];
3595	h__[i__ + en * h_dim1] = q * h__[i__ + en * h_dim1] - p * zz;
3596	/* L300: */
3597	}
3598
3599	/* .......... accumulate transformations .......... */
3600	i__1 = *igh;
3601	for (i__ = *low; i__ <= i__1; ++i__)
3602	{
3603	zz = z__[i__ + na * z_dim1];
3604	z__[i__ + na * z_dim1] = q * zz + p * z__[i__ + en * z_dim1];
3605	z__[i__ + en * z_dim1] = q * z__[i__ + en * z_dim1] - p * zz;
3606	/* L310: */
3607	}
3608	goto L330;
3609
3610	/* .......... complex pair .......... */
3611	L320:
3612	wr[na] = x + p;
3613	wr[en] = x + p;
3614	wi[na] = zz;
3615	wi[en] = -zz;
3616	L330:
3617	en = enm2;
3618	goto L60;
3619
3620	/* .......... all roots found. backsubstitute to find vectors of upper triangular form .......... */
3621	L340:
3622	if (norm == 0.0)
3623	goto L1001;
3624
3625	/* .......... for en=n step -1 until 1 do -- .......... */
3626	i__1 = *n;
3627	for (nn = 1; nn <= i__1; ++nn)
3628	{
3629	en = *n + 1 - nn;
3630	p = wr[en];
3631	q = wi[en];
3632	na = en - 1;
3633	if (q < 0.)
3634	goto L710;
3635	else if (q == 0)
3636	goto L600;
3637	else
3638	goto L800;
3639
3640	/* .......... real vector .......... */
3641	L600:
3642	m = en;
3643	h__[en + en * h_dim1] = 1.0;
3644	if (na == 0)
3645	goto L800;
3646
3647	/* .......... for i=en-1 step -1 until 1 do -- .......... */
3648	i__2 = na;
3649	for (ii = 1; ii <= i__2; ++ii)
3650	{
3651	i__ = en - ii;
3652	w = h__[i__ + i__ * h_dim1] - p;
3653	r__ = 0.0;
3654
3655	i__3 = en;
3656	for (j = m; j <= i__3; ++j)
3657	{
3658	/* L610: */
3659	r__ += h__[i__ + j * h_dim1] * h__[j + en * h_dim1];
3660	}
3661
3662	if (wi[i__] >= 0.0)
3663	goto L630;
3664	zz = w;
3665	s = r__;
3666	goto L700;
3667	L630:
3668	m = i__;
3669	if (wi[i__] != 0.0)
3670	goto L640;
3671	t = w;
3672	if (t != 0.0)
3673	goto L635;
3674	tst1 = norm;
3675	t = tst1;
3676	L632:
3677	t *= 0.01;
3678	tst2 = norm + t;
3679	if (tst2 > tst1)
3680	goto L632;
3681	L635:
3682	h__[i__ + en * h_dim1] = -r__ / t;
3683	goto L680;
3684
3685	/* .......... solve real equations .......... */
3686	L640:
3687	x = h__[i__ + (i__ + 1) * h_dim1];
3688	y = h__[i__ + 1 + i__ * h_dim1];
3689	q = (wr[i__] - p) * (wr[i__] - p) + wi[i__] * wi[i__];
3690	t = (x * s - zz * r__) / q;
3691	h__[i__ + en * h_dim1] = t;
3692	if (abs(x) <= abs(zz))
3693	goto L650;
3694	h__[i__ + 1 + en * h_dim1] = (-r__ - w * t) / x;
3695	goto L680;
3696	L650:
3697	h__[i__ + 1 + en * h_dim1] = (-s - y * t) / zz;
3698
3699	/* .......... overflow control .......... */
3700	L680:
3701	t = (d__1 = h__[i__ + en * h_dim1], abs(d__1));
3702	if (t == 0.0)
3703	goto L700;
3704	tst1 = t;
3705	tst2 = tst1 + 1.0 / tst1;
3706	if (tst2 > tst1)
3707	goto L700;
3708	i__3 = en;
3709	for (j = i__; j <= i__3; ++j)
3710	{
3711	h__[j + en * h_dim1] /= t;
3712	/* L690: */
3713	}
3714
3715	L700:
3716	;
3717	}
3718
3719	/* .......... end real vector .......... */
3720	goto L800;
3721
3722	/* .......... complex vector .......... */
3723	L710:
3724	m = na;
3725
3726	/* .......... last vector component chosen imaginary so that eigenvector matrix is triangular .......... */
3727	if ((d__1 = h__[en + na * h_dim1], abs(d__1)) <= (d__2 = h__[na + en *
3728	h_dim1], abs(d__2)))
3729	goto L720;
3730	h__[na + na * h_dim1] = q / h__[en + na * h_dim1];
3731	h__[na + en * h_dim1] = -(h__[en + en * h_dim1] - p) / h__[en + na * h_dim1];
3732	goto L730;
3733	L720:
3734	d__1 = -h__[na + en * h_dim1];
3735	d__2 = h__[na + na * h_dim1] - p;
3736	cdiv_(&c_b49, &d__1, &d__2, &q, &h__[na + na * h_dim1], &h__[na + en *
3737	h_dim1]);
3738	L730:
3739	h__[en + na * h_dim1] = 0.0;
3740	h__[en + en * h_dim1] = 1.0;
3741	enm2 = na - 1;
3742	if (enm2 == 0)
3743	goto L800;
3744
3745	/* .......... for i=en-2 step -1 until 1 do -- .......... */
3746	i__2 = enm2;
3747	for (ii = 1; ii <= i__2; ++ii)
3748	{
3749	i__ = na - ii;
3750	w = h__[i__ + i__ * h_dim1] - p;
3751	ra = 0.0;
3752	sa = 0.0;
3753
3754	i__3 = en;
3755	for (j = m; j <= i__3; ++j)
3756	{
3757	ra += h__[i__ + j * h_dim1] * h__[j + na * h_dim1];
3758	sa += h__[i__ + j * h_dim1] * h__[j + en * h_dim1];
3759	/* L760: */
3760	}
3761
3762	if (wi[i__] >= 0.0)
3763	goto L770;
3764	zz = w;
3765	r__ = ra;
3766	s = sa;
3767	goto L795;
3768	L770:
3769	m = i__;
3770	if (wi[i__] != 0.0)
3771	goto L780;
3772	d__1 = -ra;
3773	d__2 = -sa;
3774	cdiv_(&d__1, &d__2, &w, &q, &h__[i__ + na * h_dim1], &h__[i__ + en * h_dim1]);
3775	goto L790;
3776
3777	/* .......... solve complex equations .......... */
3778	L780:
3779	x = h__[i__ + (i__ + 1) * h_dim1];
3780	y = h__[i__ + 1 + i__ * h_dim1];
3781	vr = (wr[i__] - p) * (wr[i__] - p) + wi[i__] * wi[i__] - q * q;
3782	vi = (wr[i__] - p) * 2.0 * q;
3783	if (vr != 0.0 \|\| vi != 0.0)
3784	goto L784;
3785	tst1 = norm * (abs(w) + abs(q) + abs(x) + abs(y) + abs(zz));
3786	vr = tst1;
3787	L783:
3788	vr *= 0.01;
3789	tst2 = tst1 + vr;
3790	if (tst2 > tst1)
3791	goto L783;
3792	L784:
3793	d__1 = x * r__ - zz * ra + q * sa;
3794	d__2 = x * s - zz * sa - q * ra;
3795	cdiv_(&d__1, &d__2, &vr, &vi, &h__[i__ + na * h_dim1], &h__[i__ + en * h_dim1]);
3796	if (abs(x) <= abs(zz) + abs(q))
3797	goto L785;
3798	h__[i__ + 1 + na * h_dim1] = (-ra - w * h__[i__ + na * h_dim1] + q * h__[i__ + en * h_dim1]) / x;
3799	h__[i__ + 1 + en * h_dim1] = (-sa - w * h__[i__ + en * h_dim1] - q * h__[i__ + na * h_dim1]) / x;
3800	goto L790;
3801	L785:
3802	d__1 = -r__ - y * h__[i__ + na * h_dim1];
3803	d__2 = -s - y * h__[i__ + en * h_dim1];
3804	cdiv_(&d__1, &d__2, &zz, &q, &h__[i__ + 1 + na * h_dim1], &h__[i__ + 1 + en * h_dim1]);
3805
3806	/* .......... overflow control .......... */
3807	L790:
3808	/* Computing MAX */
3809	d__3 = (d__1 = h__[i__ + na * h_dim1], abs(d__1)), d__4 = (d__2 = h__[i__ + en * h_dim1], abs(d__2));
3810	t = max(d__3,d__4);
3811	if (t == 0.0)
3812	goto L795;
3813	tst1 = t;
3814	tst2 = tst1 + 1.0 / tst1;
3815	if (tst2 > tst1)
3816	goto L795;
3817	i__3 = en;
3818	for (j = i__; j <= i__3; ++j)
3819	{
3820	h__[j + na * h_dim1] /= t;
3821	h__[j + en * h_dim1] /= t;
3822	/* L792: */
3823	}
3824	L795:
3825	;
3826	}
3827	/* .......... end complex vector .......... */
3828	L800:
3829	;
3830	}
3831	/* .......... end back substitution vectors of isolated roots .......... */
3832	i__1 = *n;
3833	for (i__ = 1; i__ <= i__1; ++i__)
3834	{
3835	if (i__ >= low && i__ <= igh)
3836	goto L840;
3837	i__2 = *n;
3838	for (j = i__; j <= i__2; ++j)
3839	{
3840	/* L820: */
3841	z__[i__ + j * z_dim1] = h__[i__ + j * h_dim1];
3842	}
3843	L840:
3844	;
3845	}
3846
3847	/* .......... multiply by transformation matrix to give vectors of original full matrix. */
3848	/* for j=n step -1 until low do -- .......... */
3849	i__1 = *n;
3850	for (jj = *low; jj <= i__1; ++jj)
3851	{
3852	j = n + low - jj;
3853	m = min(j,*igh);
3854
3855	i__2 = *igh;
3856	for (i__ = *low; i__ <= i__2; ++i__)
3857	{
3858	zz = 0.0;
3859	i__3 = m;
3860	for (k = *low; k <= i__3; ++k)
3861	{
3862	/* L860: */
3863	zz += z__[i__ + k * z_dim1] * h__[k + j * h_dim1];
3864	}
3865
3866	z__[i__ + j * z_dim1] = zz;
3867	/* L880: */
3868	}
3869	}
3870
3871	goto L1001;
3872	/* .......... set error -- all eigenvalues have not converged after 30n iterations .......... /
3873	L1000:
3874	*ierr = en;
3875	L1001:
3876	return 0;
3877
3878	}
3879	/* end f2c version of code */
3880	#endif
3881
3882	}
3883
3884
3885
3886
3887
3888	MrBFlt IncompleteBetaFunction (MrBFlt alpha, MrBFlt beta, MrBFlt x)
3889
3890	{
3891
3892	MrBFlt bt, gm1, gm2, gm3, temp;
3893
3894	if (x < 0.0 \|\| x > 1.0)
3895	{
3896	MrBayesPrint ("%s Error: Problem in IncompleteBetaFunction.\n", spacer);
3897	exit (0);
3898	}
3899	if (fabs(x) < ETA \|\| fabs(x-1.0)<ETA) /* x == 0.0 \|\| x == 1.0 */
3900	{
3901	bt = 0.0;
3902	}
3903	else
3904	{
3905	gm1 = LnGamma (alpha + beta);
3906	gm2 = LnGamma (alpha);
3907	gm3 = LnGamma (beta);
3908	temp = gm1 - gm2 - gm3 + (alpha) * log(x) + (beta) * log(1.0 - x);
3909	bt = exp(temp);
3910	}
3911	if (x < (alpha + 1.0)/(alpha + beta + 2.0))
3912	return (bt * BetaCf(alpha, beta, x) / alpha);
3913	else
3914	return (1.0 - bt * BetaCf(beta, alpha, 1.0-x) / beta);
3915
3916	}
3917
3918
3919
3920
3921
3922	/*---------------------------------------------------------------------------------
3923	\|
3924	\| IncompleteGamma
3925	\|
3926	\| Returns the incomplete gamma ratio I(x,alpha) where x is the upper
3927	\| limit of the integration and alpha is the shape parameter. Returns (-1)
3928	\| if in error.
3929	\|
3930	\| Bhattacharjee, G. P. 1970. The incomplete gamma integral. Applied
3931	\| Statistics, 19:285-287 (AS32)
3932	\|
3933	---------------------------------------------------------------------------------*/
3934	MrBFlt IncompleteGamma (MrBFlt x, MrBFlt alpha, MrBFlt LnGamma_alpha)
3935
3936	{
3937
3938	int i;
3939	MrBFlt p = alpha, g = LnGamma_alpha,
3940	accurate = 1e-8, overflow = 1e30,
3941	factor, gin = 0.0, rn = 0.0, a = 0.0, b = 0.0, an = 0.0,
3942	dif = 0.0, term = 0.0, pn[6];
3943
3944	if (fabs(x) < ETA)
3945	return (0.0);
3946	if (x < 0 \|\| p <= 0)
3947	return (-1.0);
3948
3949	factor = exp(p*log(x)-x-g);
3950	if (x>1 && x>=p)
3951	goto l30;
3952	gin = 1.0;
3953	term = 1.0;
3954	rn = p;
3955	l20:
3956	rn++;
3957	term *= x/rn;
3958	gin += term;
3959	if (term > accurate)
3960	goto l20;
3961	gin *= factor/p;
3962	goto l50;
3963	l30:
3964	a = 1.0-p;
3965	b = a+x+1.0;
3966	term = 0.0;
3967	pn[0] = 1.0;
3968	pn[1] = x;
3969	pn[2] = x+1;
3970	pn[3] = x*b;
3971	gin = pn[2]/pn[3];
3972	l32:
3973	a++;
3974	b += 2.0;
3975	term++;
3976	an = a*term;
3977	for (i=0; i<2; i++)
3978	pn[i+4] = bpn[i+2]-anpn[i];
3979	if (fabs(pn[5]) < ETA)
3980	goto l35;
3981	rn = pn[4]/pn[5];
3982	dif = fabs(gin-rn);
3983	if (dif>accurate)
3984	goto l34;
3985	if (dif<=accurate*rn)
3986	goto l42;
3987	l34:
3988	gin = rn;
3989	l35:
3990	for (i=0; i<4; i++)
3991	pn[i] = pn[i+2];
3992	if (fabs(pn[4]) < overflow)
3993	goto l32;
3994	for (i=0; i<4; i++)
3995	pn[i] /= overflow;
3996	goto l32;
3997	l42:
3998	gin = 1.0-factor*gin;
3999	l50:
4000	return (gin);
4001
4002	}
4003
4004
4005
4006
4007
4008	/*---------------------------------------------------------------------------------
4009	\|
4010	\| InvertMatrix
4011	\|
4012	\| Calculates aInv = a^{-1} using LU-decomposition. The input matrix a is
4013	\| destroyed in the process. The program returns an error if the matrix is
4014	\| singular. col and indx are work vectors.
4015	\|
4016	---------------------------------------------------------------------------------*/
4017	int InvertMatrix (int dim, MrBFlt *a, MrBFlt col, int indx, MrBFlt *aInv)
4018
4019	{
4020
4021	int rc, i, j;
4022
4023	rc = LUDecompose (dim, a, col, indx, (MrBFlt *)NULL);
4024	if (rc == FALSE)
4025	{
4026	for (j = 0; j < dim; j++)
4027	{
4028	for (i = 0; i < dim; i++)
4029	col[i] = 0.0;
4030	col[j] = 1.0;
4031	LUBackSubstitution (dim, a, indx, col);
4032	for (i = 0; i < dim; i++)
4033	aInv[i][j] = col[i];
4034	}
4035	}
4036
4037	return (rc);
4038
4039	}
4040
4041
4042
4043
4044
4045	/*---------------------------------------------------------------------------------
4046	\|
4047	\| LBinormal
4048	\|
4049	\| L(h1,h2,r) = prob(x>h1, y>h2), where x and y are standard binormal,
4050	\| with r=corr(x,y), error < 2e-7.
4051	\|
4052	\| Drezner Z., and G.O. Wesolowsky (1990) On the computation of the
4053	\| bivariate normal integral. J. Statist. Comput. Simul. 35:101-107.
4054	\|
4055	---------------------------------------------------------------------------------*/
4056	MrBFlt LBinormal (MrBFlt h1, MrBFlt h2, MrBFlt r)
4057
4058	{
4059
4060	int i;
4061	MrBFlt x[]={0.04691008, 0.23076534, 0.5, 0.76923466, 0.95308992};
4062	MrBFlt w[]={0.018854042, 0.038088059, 0.0452707394,0.038088059,0.018854042};
4063	MrBFlt Lh=0.0, r1, r2, r3, rr, aa, ab, h3, h5, h6, h7, h12, temp1, temp2, exp1, exp2;
4064
4065	h12 = (h1 * h1 + h2 * h2) / 2.0;
4066	if (fabs(r) >= 0.7)
4067	{
4068	r2 = 1.0 - r * r;
4069	r3 = sqrt(r2);
4070	if (r < 0)
4071	h2 *= -1;
4072	h3 = h1 * h2;
4073	h7 = exp(-h3 / 2.0);
4074	if (fabs(r-1.0)>ETA) /* fabs(r) != 1.0 */
4075	{
4076	h6 = fabs(h1-h2);
4077	h5 = h6 * h6 / 2.0;
4078	h6 /= r3;
4079	aa = 0.5 - h3 / 8;
4080	ab = 3.0 - 2.0 * aa * h5;
4081	temp1 = -h5 / r2;
4082	if (temp1 < -100.0)
4083	exp1 = 0.0;
4084	else
4085	exp1 = exp(temp1);
4086	Lh = 0.13298076 * h6 * ab * (1.0 - CdfNormal(h6)) - exp1 * (ab + aa * r2) * 0.053051647;
4087	for (i=0; i<5; i++)
4088	{
4089	r1 = r3 * x[i];
4090	rr = r1 * r1;
4091	r2 = sqrt(1.0 - rr);
4092	temp1 = -h5 / rr;
4093	if (temp1 < -100.0)
4094	exp1 = 0.0;
4095	else
4096	exp1 = exp(temp1);
4097	temp2 = -h3 / (1.0 + r2);
4098	if (temp2 < -100.0)
4099	exp2 = 0.0;
4100	else
4101	exp2 = exp(temp2);
4102	Lh -= w[i] * exp1 * (exp2 / r2 / h7 - 1.0 - aa * rr);
4103	}
4104	}
4105	if (r > 0)
4106	Lh = Lh * r3 * h7 + (1.0 - CdfNormal(MAX(h1, h2)));
4107	else if (r<0)
4108	Lh = (h1 < h2 ? CdfNormal(h2) - CdfNormal(h1) : 0) - Lh * r3 * h7;
4109	}
4110	else
4111	{
4112	h3 = h1 * h2;
4113	if (fabs(r)>ETA)
4114	{
4115	for (i=0; i<5; i++)
4116	{
4117	r1 = r * x[i];
4118	r2 = 1.0 - r1 * r1;
4119	temp1 = (r1 * h3 - h12) / r2;
4120	if (temp1 < -100.0)
4121	exp1 = 0.0;
4122	else
4123	exp1 = exp(temp1);
4124	Lh += w[i] * exp1 / sqrt(r2);
4125	}
4126	}
4127	Lh = (1.0 - CdfNormal(h1)) * (1.0 - CdfNormal(h2)) + r * Lh;
4128	}
4129	return (Lh);
4130
4131	}
4132
4133
4134
4135
4136
4137	/*---------------------------------------------------------------------------------
4138	\|
4139	\| LnFactorial: Calculates the log of the factorial for an integer
4140	\|
4141	---------------------------------------------------------------------------------*/
4142	MrBFlt LnFactorial (int value)
4143	{
4144	int i;
4145	MrBFlt result;
4146
4147	result = 0.0;
4148
4149	for (i = 2; i<=value; i++)
4150	result += log(i);
4151
4152	return result;
4153	}
4154
4155
4156
4157
4158
4159	/*---------------------------------------------------------------------------------
4160	\|
4161	\| LnGamma
4162	\|
4163	\| Calculates the log of the gamma function. The Gamma function is equal
4164	\| to:
4165	\|
4166	\| Gamma(alp) = {integral from 0 to infinity} t^{alp-1} e^-t dt
4167	\|
4168	\| The result is accurate to 10 decimal places. Stirling's formula is used
4169	\| for the central polynomial part of the procedure.
4170	\|
4171	\| Pike, M. C. and I. D. Hill. 1966. Algorithm 291: Logarithm of the gamma
4172	\| function. Communications of the Association for Computing
4173	\| Machinery, 9:684.
4174	\|
4175	---------------------------------------------------------------------------------*/
4176	MrBFlt LnGamma (MrBFlt alp)
4177
4178	{
4179
4180	MrBFlt x = alp, f = 0.0, z;
4181
4182	if (x < 7)
4183	{
4184	f = 1.0;
4185	z = x-1.0;
4186	while (++z < 7.0)
4187	f *= z;
4188	x = z;
4189	f = -log(f);
4190	}
4191	z = 1.0 / (x*x);
4192	return (f + (x-0.5)*log(x) - x + 0.918938533204673 +
4193	(((-0.000595238095238z+0.000793650793651)z-0.002777777777778)*z +
4194	0.083333333333333)/x);
4195
4196	}
4197
4198
4199
4200
4201
4202	/* Calculate probability of a realization for exponential random variable */
4203	MrBFlt LnPriorProbExponential(MrBFlt val, MrBFlt *params)
4204	{
4205	return log(params[0]) - params[0] * val;
4206	}
4207
4208
4209
4210
4211
4212	/* Calculate probability of a realization for a fixed variable */
4213	MrBFlt LnPriorProbFix(MrBFlt val, MrBFlt *params)
4214	{
4215	if (AreDoublesEqual(val, params[0], 0.00001) == YES)
4216	return 0.0;
4217	else
4218	return NEG_INFINITY;
4219	}
4220
4221
4222
4223
4224
4225	/* Calculate probability of a realization for gamma random variable */
4226	MrBFlt LnPriorProbGamma(MrBFlt val, MrBFlt *params)
4227	{
4228	return (params[0] - 1) * log(val) + params[0] * log(params[1]) - params[1] * val - LnGamma(params[0]);
4229	}
4230
4231
4232
4233
4234
4235	/* Calculate probability of a realization for lognormal random variable */
4236	MrBFlt LnPriorProbLognormal(MrBFlt val, MrBFlt *params)
4237	{
4238	MrBFlt z;
4239
4240	z = (log(val) - params[0]) / params[1];
4241
4242	return -log(params[1] * val * sqrt(2.0 * PI)) - z * z / 2.0;
4243	}
4244
4245
4246
4247
4248
4249	/* Calculate probability of a realization for normal random variable */
4250	MrBFlt LnPriorProbNormal(MrBFlt val, MrBFlt *params)
4251	{
4252	MrBFlt z;
4253
4254	z = (val - params[0]) / params[1];
4255
4256	return -log(params[1] * sqrt(2.0 * PI)) - z * z / 2.0;
4257	}
4258
4259
4260
4261
4262
4263	/* Calculate probability of a realization for truncated (only positive values) normal random variable */
4264	MrBFlt LnPriorProbTruncatedNormal(MrBFlt val, MrBFlt *params)
4265	{
4266	MrBFlt z, z_0, normConst;
4267
4268	z = (val - params[0]) / params[1];
4269
4270	z_0 = (0.0 - params[0]) / params[1];
4271	normConst = CdfNormal(z_0);
4272
4273	return -log(params[1] * sqrt(2.0 * PI)) - z * z / 2.0 - log(normConst);
4274	}
4275
4276
4277
4278
4279
4280	/* Calculate probability of a realization for uniform random variable */
4281	MrBFlt LnPriorProbUniform(MrBFlt val, MrBFlt *params)
4282	{
4283	return - log(params[1] - params[0]);
4284	}
4285
4286
4287
4288
4289
4290	/* Calculate probability ratio of realizations for exponential random variable */
4291	MrBFlt LnProbRatioExponential(MrBFlt newX, MrBFlt oldX, MrBFlt *params)
4292	{
4293	return params[0] * (oldX - newX);
4294	}
4295
4296
4297
4298
4299
4300	/* Calculate probability ratio of realizations for gamma random variable */
4301	MrBFlt LnProbRatioGamma(MrBFlt newX, MrBFlt oldX, MrBFlt *params)
4302	{
4303	return (params[1] - 1.0) * (log(newX) - log(oldX)) - params[0] * (newX - oldX);
4304	}
4305
4306
4307
4308
4309
4310	/* Calculate probability ratio of realizations for log normal random variable */
4311	MrBFlt LnProbRatioLognormal (MrBFlt newX, MrBFlt oldX, MrBFlt *params)
4312	{
4313	MrBFlt newZ, oldZ;
4314
4315	newZ = (log(newX) - params[0]) / params[1];
4316	oldZ = (log(oldX) - params[0]) / params[1];
4317
4318	return (oldZ * oldZ - newZ * newZ) / 2.0 + log(oldX) - log(newX);
4319	}
4320
4321
4322
4323
4324
4325	/* Calculate probability ratio of realizations for normal random variable */
4326	MrBFlt LnProbRatioNormal (MrBFlt newX, MrBFlt oldX, MrBFlt *params)
4327	{
4328	MrBFlt newZ, oldZ;
4329
4330	newZ = (newX - params[0]) / params[1];
4331	oldZ = (oldX - params[0]) / params[1];
4332
4333	return (oldZ * oldZ - newZ * newZ) / 2.0;
4334	}
4335
4336
4337
4338
4339
4340	/* Calculate probability ratio of realizations for truncated normal random variable */
4341	MrBFlt LnProbRatioTruncatedNormal (MrBFlt newX, MrBFlt oldX, MrBFlt *params)
4342	{
4343	MrBFlt newZ, oldZ;
4344
4345	if (newX <= 0.0)
4346	return NEG_INFINITY;
4347	else if (oldX <= 0.0)
4348	return (POS_INFINITY);
4349
4350	newZ = (newX - params[0]) / params[1];
4351	oldZ = (oldX - params[0]) / params[1];
4352
4353	return (oldZ * oldZ - newZ * newZ) / 2.0;
4354	}
4355
4356
4357
4358
4359
4360	/* Calculate probability ratio of realizations for uniform random variable */
4361	MrBFlt LnProbRatioUniform (MrBFlt newX, MrBFlt oldX, MrBFlt *params)
4362	{
4363	return 0.0;
4364	}
4365
4366
4367
4368
4369
4370	/* Log probability for a value drawn from a lognormal distribution; parameters are
4371	mean and variance of value (not of log value) */
4372	MrBFlt LnProbTK02LogNormal (MrBFlt mean, MrBFlt var, MrBFlt x)
4373
4374	{
4375	MrBFlt z, lnProb, mu, sigma;
4376
4377	sigma = sqrt(log(1.0 + (var / (mean*mean))));
4378	mu = log(mean) - sigma * sigma / 2.0;
4379
4380	z = (log(x) - mu) / sigma;
4381
4382	lnProb = - log (x * sigma * sqrt (2.0 * PI)) - (z*z / 2.0);
4383
4384	return lnProb;
4385	}
4386
4387
4388
4389
4390
4391	/* Log probability for a value drawn from a gamma distribution */
4392	MrBFlt LnProbGamma (MrBFlt alpha, MrBFlt beta, MrBFlt x)
4393
4394	{
4395	MrBFlt lnProb;
4396
4397	lnProb = (alpha-1.0)log(x) + alphalog(beta) - x*beta - LnGamma(alpha);
4398
4399	return lnProb;
4400	}
4401
4402
4403
4404
4405
4406	/* Log probability for a value drawn from a lognormal distribution */
4407	MrBFlt LnProbLogNormal (MrBFlt exp, MrBFlt sd, MrBFlt x)
4408
4409	{
4410	MrBFlt z, lnProb;
4411
4412	z = (log(x) - exp) / sd;
4413
4414	lnProb = - log (x * sd * sqrt (2.0 * PI)) - (z*z / 2.0);
4415
4416	return lnProb;
4417	}
4418
4419
4420
4421
4422
4423	/* Log probability for a value drawn from a scaled gamma distribution */
4424	MrBFlt LnProbScaledGamma (MrBFlt alpha, MrBFlt x)
4425
4426	{
4427	MrBFlt lnProb;
4428
4429	lnProb = (alpha - 1.0) * log(x) - LnGamma(alpha) + alphalog(alpha) - xalpha;
4430
4431	return lnProb;
4432	}
4433
4434
4435
4436
4437
4438	/* Log probability for a value drawn from a truncated gamma distribution */
4439	MrBFlt LnProbTruncGamma (MrBFlt alpha, MrBFlt beta, MrBFlt x, MrBFlt min, MrBFlt max)
4440
4441	{
4442	MrBFlt lnProb;
4443
4444	lnProb = (alpha-1.0)log(x) + alphalog(beta) - x*beta - LnGamma(alpha);
4445
4446	lnProb -= log (IncompleteGamma (maxbeta, alpha, LnGamma(alpha)) - IncompleteGamma (minbeta, alpha, LnGamma(alpha)));
4447
4448	return lnProb;
4449	}
4450
4451
4452
4453
4454
4455	/* Log ratio for two values drawn from a lognormal distribution */
4456	MrBFlt LnRatioTK02LogNormal (MrBFlt mean, MrBFlt var, MrBFlt xNew, MrBFlt xOld)
4457
4458	{
4459	MrBFlt newZ, oldZ, mu, sigma;
4460
4461	sigma = sqrt(log(1.0 + (var / (mean*mean))));
4462	mu = log(mean) - sigma * sigma / 2.0;
4463
4464	newZ = (log(xNew) - mu) / sigma;
4465	oldZ = (log(xOld) - mu) / sigma;
4466
4467	return (oldZ * oldZ - newZ * newZ) / 2.0 + log(xOld) - log(xNew);
4468	}
4469
4470
4471
4472
4473
4474	/* Log ratio for two values drawn from a lognormal distribution */
4475	MrBFlt LnRatioLogNormal (MrBFlt exp, MrBFlt sd, MrBFlt xNew, MrBFlt xOld)
4476
4477	{
4478	MrBFlt newZ, oldZ;
4479
4480	newZ = (log(xNew) - exp) / sd;
4481	oldZ = (log(xOld) - exp) / sd;
4482
4483	return (oldZ * oldZ - newZ * newZ) / 2.0 + log(xOld) - log(xNew);
4484	}
4485
4486
4487
4488
4489
4490	/*---------------------------------------------------------------------------------
4491	\|
4492	\| LogBase2Plus1
4493	\|
4494	\| This function is called from ComputeMatrixExponential.
4495	\|
4496	---------------------------------------------------------------------------------*/
4497	int LogBase2Plus1 (MrBFlt x)
4498
4499	{
4500
4501	int j = 0;
4502
4503	while(x > 1.0 - 1.0e-07)
4504	{
4505	x /= 2.0;
4506	j++;
4507	}
4508
4509	return (j);
4510
4511	}
4512
4513
4514
4515
4516
4517	/*---------------------------------------------------------------------------------
4518	\|
4519	\| LogNormalRandomVariable
4520	\|
4521	\| Draw a random variable from a lognormal distribution.
4522	\|
4523	---------------------------------------------------------------------------------*/
4524	MrBFlt LogNormalRandomVariable (MrBFlt mean, MrBFlt sd, SafeLong *seed)
4525
4526	{
4527
4528	MrBFlt x;
4529
4530	x = PointNormal(RandomNumber(seed));
4531
4532	x*= sd;
4533	x += mean;
4534
4535	return exp(x);
4536	}
4537
4538
4539
4540
4541
4542	/*---------------------------------------------------------------------------------
4543	\|
4544	\| LUBackSubstitution
4545	\|
4546	\| Back substitute into an LU-decomposed matrix.
4547	\|
4548	---------------------------------------------------------------------------------*/
4549	void LUBackSubstitution (int dim, MrBFlt *a, int indx, MrBFlt *b)
4550
4551	{
4552
4553	int i, ip, j, ii = -1;
4554	MrBFlt sum;
4555
4556	for (i=0; i<dim; i++)
4557	{
4558	ip = indx[i];
4559	sum = b[ip];
4560	b[ip] = b[i];
4561	if (ii >= 0)
4562	{
4563	for (j=ii; j<=i-1; j++)
4564	sum -= a[i][j] * b[j];
4565	}
4566	else if (fabs(sum)>ETA)
4567	ii = i;
4568	b[i] = sum;
4569	}
4570	for (i=dim-1; i>=0; i--)
4571	{
4572	sum = b[i];
4573	for (j=i+1; j<dim; j++)
4574	sum -= a[i][j] * b[j];
4575	b[i] = sum / a[i][i];
4576	}
4577
4578	}
4579
4580
4581
4582
4583
4584	/*---------------------------------------------------------------------------------
4585	\|
4586	\| LUDecompose
4587	\|
4588	\| Calculate the LU-decomposition of the matrix a. The matrix a is replaced.
4589	\|
4590	---------------------------------------------------------------------------------*/
4591	int LUDecompose (int dim, MrBFlt *a, MrBFlt vv, int indx, MrBFlt pd)
4592
4593	{
4594
4595	int i, imax=0, j, k;
4596	MrBFlt big, dum, sum, temp, d;
4597
4598	d = 1.0;
4599	for (i=0; i<dim; i++)
4600	{
4601	big = 0.0;
4602	for (j = 0; j < dim; j++)
4603	{
4604	if ((temp = fabs(a[i][j])) > big)
4605	big = temp;
4606	}
4607	if (fabs(big)<ETA)
4608	{
4609	MrBayesPrint ("%s Error: Problem in LUDecompose\n", spacer);
4610	return (ERROR);
4611	}
4612	vv[i] = 1.0 / big;
4613	}
4614	for (j=0; j<dim; j++)
4615	{
4616	for (i = 0; i < j; i++)
4617	{
4618	sum = a[i][j];
4619	for (k = 0; k < i; k++)
4620	sum -= a[i][k] * a[k][j];
4621	a[i][j] = sum;
4622	}
4623	big = 0.0;
4624	for (i=j; i<dim; i++)
4625	{
4626	sum = a[i][j];
4627	for (k = 0; k < j; k++)
4628	sum -= a[i][k] * a[k][j];
4629	a[i][j] = sum;
4630	dum = vv[i] * fabs(sum);
4631	if (dum >= big)
4632	{
4633	big = dum;
4634	imax = i;
4635	}
4636	}
4637	if (j != imax)
4638	{
4639	for (k=0; k<dim; k++)
4640	{
4641	dum = a[imax][k];
4642	a[imax][k] = a[j][k];
4643	a[j][k] = dum;
4644	}
4645	d = -d;
4646	vv[imax] = vv[j];
4647	}
4648	indx[j] = imax;
4649	if (fabs(a[j][j])<ETA)
4650	a[j][j] = TINY;
4651	if (j != dim - 1)
4652	{
4653	dum = 1.0 / (a[j][j]);
4654	for (i=j+1; i<dim; i++)
4655	a[i][j] *= dum;
4656	}
4657	}
4658	if (pd != NULL)
4659	*pd = d;
4660
4661	return (NO_ERROR);
4662
4663	}
4664
4665
4666
4667
4668
4669	/*---------------------------------------------------------------------------------
4670	\|
4671	\| MultiplyMatrices
4672	\|
4673	\| Multiply matrix a by matrix b and put the results in matrix result.
4674	\|
4675	---------------------------------------------------------------------------------*/
4676	void MultiplyMatrices (int dim, MrBFlt a, MrBFlt b, MrBFlt **result)
4677
4678	{
4679
4680	register int i, j, k;
4681	MrBFlt **temp;
4682
4683	temp = AllocateSquareDoubleMatrix (dim);
4684
4685	for (i=0; i<dim; i++)
4686	{
4687	for (j=0; j<dim; j++)
4688	{
4689	temp[i][j] = 0.0;
4690	for (k=0; k<dim; k++)
4691	{
4692	temp[i][j] += a[i][k] * b[k][j];
4693	}
4694	}
4695	}
4696	for (i=0; i<dim; i++)
4697	{
4698	for (j=0; j<dim; j++)
4699	{
4700	result[i][j] = temp[i][j];
4701	}
4702	}
4703
4704	FreeSquareDoubleMatrix (temp);
4705
4706	}
4707
4708
4709
4710
4711
4712	/*---------------------------------------------------------------------------------
4713	\|
4714	\| MultiplyMatrixByScalar
4715	\|
4716	\| Multiply the elements of matrix a by a scalar.
4717	\|
4718	---------------------------------------------------------------------------------*/
4719	void MultiplyMatrixByScalar (int dim, MrBFlt a, MrBFlt scalar, MrBFlt result)
4720
4721	{
4722
4723	int row, col;
4724
4725	for (row=0; row<dim; row++)
4726	for (col=0; col<dim; col++)
4727	result[row][col] = a[row][col] * scalar;
4728
4729	}
4730
4731
4732
4733
4734
4735	/*---------------------------------------------------------------------------------
4736	\|
4737	\| MultiplyMatrixNTimes
4738	\|
4739	---------------------------------------------------------------------------------*/
4740	int MultiplyMatrixNTimes (int dim, MrBFlt Mat, int power, MrBFlt Result)
4741
4742	{
4743
4744	register int i, j;
4745	int k, numSquares, numRemaining;
4746	MrBFlt TempIn, TempOut;
4747
4748	if (power < 0)
4749	{
4750	MrBayesPrint ("%s Error: Power cannot be a negative number.\n", spacer);
4751	return (ERROR);
4752	}
4753	else if (power == 0)
4754	{
4755	for (i=0; i<dim; i++)
4756	for (j=0; j<dim; j++)
4757	Result[i][j] = 1.0;
4758	}
4759	else
4760	{
4761	TempIn = AllocateSquareDoubleMatrix (dim);
4762	TempOut = AllocateSquareDoubleMatrix (dim);
4763
4764	/* how many times can I multiply the matrices together */
4765	numSquares = 0;
4766	while (pow (2.0, (MrBFlt)(numSquares)) < power)
4767	numSquares++;
4768	numRemaining = power - (int)(pow(2.0, (MrBFlt)(numSquares)));
4769
4770	/* now, multiply matrix by power of 2's */
4771	CopyDoubleMatrices (dim, Mat, TempIn);
4772	for (k=0; k<numSquares; k++)
4773	{
4774	MultiplyMatrices (dim, TempIn, TempIn, TempOut);
4775	CopyDoubleMatrices (dim, TempOut, TempIn);
4776	}
4777
4778	/* TempIn is Mat^numSquares. Now, multiply it by Mat numRemaining times */
4779	for (k=0; k<numSquares; k++)
4780	{
4781	MultiplyMatrices (dim, TempIn, Mat, TempOut);
4782	CopyDoubleMatrices (dim, TempOut, TempIn);
4783	}
4784
4785	/* copy result */
4786	CopyDoubleMatrices (dim, TempIn, Result);
4787
4788	FreeSquareDoubleMatrix (TempIn);
4789	FreeSquareDoubleMatrix (TempOut);
4790	}
4791
4792	return (NO_ERROR);
4793
4794	}
4795
4796
4797
4798
4799
4800	/*---------------------------------------------------------------------------------
4801	\|
4802	\| PointChi2
4803	\|
4804	\| Returns z so that Prob(x < z) = prob where x is Chi2 distributed with df=v.
4805	\| Returns -1 if in error. 0.000002 < prob < 0.999998.
4806	\|
4807	---------------------------------------------------------------------------------*/
4808	MrBFlt PointChi2 (MrBFlt prob, MrBFlt v)
4809
4810	{
4811
4812	MrBFlt e = 0.5e-6, aa = 0.6931471805, p = prob, g,
4813	xx, c, ch, a = 0.0, q = 0.0, p1 = 0.0, p2 = 0.0, t = 0.0,
4814	x = 0.0, b = 0.0, s1, s2, s3, s4, s5, s6;
4815
4816	if (p < 0.000002 \|\| p > 0.999998 \|\| v <= 0.0)
4817	return (-1.0);
4818	g = LnGamma (v/2.0);
4819	xx = v/2.0;
4820	c = xx - 1.0;
4821	if (v >= -1.24*log(p))
4822	goto l1;
4823	ch = pow((pxxexp(g+xx*aa)), 1.0/xx);
4824	if (ch-e<0)
4825	return (ch);
4826	goto l4;
4827	l1:
4828	if (v > 0.32)
4829	goto l3;
4830	ch = 0.4;
4831	a = log(1.0-p);
4832	l2:
4833	q = ch;
4834	p1 = 1.0+ch*(4.67+ch);
4835	p2 = ch(6.73+ch(6.66+ch));
4836	t = -0.5+(4.67+2.0ch)/p1 - (6.73+ch(13.32+3.0*ch))/p2;
4837	ch -= (1.0-exp(a+g+0.5ch+caa)*p2/p1)/t;
4838	if (fabs(q/ch-1.0)-0.01 <= 0.0)
4839	goto l4;
4840	else
4841	goto l2;
4842	l3:
4843	x = PointNormal (p);
4844	p1 = 0.222222/v;
4845	ch = vpow((xsqrt(p1)+1.0-p1), 3.0);
4846	if (ch > 2.2*v+6.0)
4847	ch = -2.0(log(1.0-p)-clog(0.5*ch)+g);
4848	l4:
4849	q = ch;
4850	p1 = 0.5*ch;
4851	if ((t = IncompleteGamma (p1, xx, g)) < 0.0)
4852	{
4853	MrBayesPrint ("%s Error: Problem in PointChi2", spacer);
4854	return (-1.0);
4855	}
4856	p2 = p-t;
4857	t = p2exp(xxaa+g+p1-c*log(ch));
4858	b = t/ch;
4859	a = 0.5t-bc;
4860	s1 = (210.0+a(140.0+a(105.0+a(84.0+a(70.0+60.0*a))))) / 420.0;
4861	s2 = (420.0+a(735.0+a(966.0+a(1141.0+1278.0a))))/2520.0;
4862	s3 = (210.0+a(462.0+a(707.0+932.0*a)))/2520.0;
4863	s4 = (252.0+a(672.0+1182.0a)+c(294.0+a(889.0+1740.0*a)))/5040.0;
4864	s5 = (84.0+264.0a+c(175.0+606.0*a)) / 2520.0;
4865	s6 = (120.0+c(346.0+127.0c)) / 5040.0;
4866	ch += t(1+0.5ts1-bc(s1-b(s2-b(s3-b(s4-b(s5-bs6))))));
4867	if (fabs(q/ch-1.0) > e)
4868	goto l4;
4869	return (ch);
4870
4871	}
4872
4873
4874
4875
4876
4877	/*---------------------------------------------------------------------------------
4878	\|
4879	\| PointNormal
4880	\|
4881	\| Returns z so That Prob{x<z} = prob where x ~ N(0,1) and
4882	\| (1e-12) < prob < 1-(1e-12). Returns (-9999) if in error.
4883	\|
4884	\| Odeh, R. E. and J. O. Evans. 1974. The percentage points of the normal
4885	\| distribution. Applied Statistics, 22:96-97 (AS70).
4886	\|
4887	\| Newer methods:
4888	\|
4889	\| Wichura, M. J. 1988. Algorithm AS 241: The percentage points of the
4890	\| normal distribution. 37:477-484.
4891	\| Beasley, JD & S. G. Springer. 1977. Algorithm AS 111: The percentage
4892	\| points of the normal distribution. 26:118-121.
4893	\|
4894	---------------------------------------------------------------------------------*/
4895	MrBFlt PointNormal (MrBFlt prob)
4896
4897	{
4898
4899	MrBFlt a0 = -0.322232431088, a1 = -1.0, a2 = -0.342242088547, a3 = -0.0204231210245,
4900	a4 = -0.453642210148e-4, b0 = 0.0993484626060, b1 = 0.588581570495,
4901	b2 = 0.531103462366, b3 = 0.103537752850, b4 = 0.0038560700634,
4902	y, z = 0, p = prob, p1;
4903
4904	p1 = (p<0.5 ? p : 1-p);
4905	if (p1<1e-20)
4906	return (-9999);
4907	y = sqrt (log(1/(p1*p1)));
4908	z = y + ((((ya4+a3)y+a2)y+a1)y+a0) / ((((yb4+b3)y+b2)y+b1)y+b0);
4909
4910	return (p<0.5 ? -z : z);
4911
4912	}
4913
4914
4915
4916
4917
4918	/*---------------------------------------------------------------------------------
4919	\|
4920	\| PrintComplexVector
4921	\|
4922	\| Prints a vector of dim complex numbers.
4923	\|
4924	---------------------------------------------------------------------------------*/
4925	void PrintComplexVector (int dim, complex *vec)
4926
4927	{
4928
4929	int i;
4930
4931	MrBayesPrint ("{");
4932	for (i = 0; i < (dim - 1); i++)
4933	{
4934	MrBayesPrint ("%lf + %lfi, ", vec[i].re, vec[i].im);
4935	if(i == 1)
4936	MrBayesPrint("\n ");
4937	}
4938	MrBayesPrint ("%lf + %lfi}\n", vec[dim - 1].re, vec[dim - 1].im);
4939
4940	}
4941
4942
4943
4944
4945
4946	/*---------------------------------------------------------------------------------
4947	\|
4948	\| PrintSquareComplexMatrix
4949	\|
4950	\| Prints a square matrix of complex numbers.
4951	\|
4952	---------------------------------------------------------------------------------*/
4953	void PrintSquareComplexMatrix (int dim, complex **m)
4954
4955	{
4956
4957	int row, col;
4958
4959	MrBayesPrint ("{");
4960	for (row = 0; row < (dim - 1); row++)
4961	{
4962	MrBayesPrint ("{");
4963	for(col = 0; col < (dim - 1); col++)
4964	{
4965	MrBayesPrint ("%lf + %lfi, ", m[row][col].re, m[row][col].im);
4966	if(col == 1)
4967	MrBayesPrint ("\n ");
4968	}
4969	MrBayesPrint ("%lf + %lfi},\n",
4970	m[row][dim - 1].re, m[row][dim - 1].im);
4971	}
4972	MrBayesPrint ("{");
4973	for (col = 0; col < (dim - 1); col++)
4974	{
4975	MrBayesPrint ("%lf + %lfi, ", m[dim - 1][col].re, m[dim - 1][col].im);
4976	if(col == 1)
4977	MrBayesPrint ("\n ");
4978	}
4979	MrBayesPrint ("%lf + %lfi}}", m[dim - 1][dim - 1].re, m[dim - 1][dim - 1].im);
4980	MrBayesPrint ("\n");
4981
4982	}
4983
4984
4985
4986
4987
4988	/*---------------------------------------------------------------------------------
4989	\|
4990	\| PrintSquareDoubleMatrix
4991	\|
4992	\| Prints a square matrix of doubles.
4993	\|
4994	---------------------------------------------------------------------------------*/
4995	void PrintSquareDoubleMatrix (int dim, MrBFlt **matrix)
4996
4997	{
4998
4999	int i, j;
5000
5001	for (i=0; i<dim; i++)
5002	{
5003	for(j=0; j<dim; j++)
5004	MrBayesPrint ("%1.6lf ", matrix[i][j]);
5005	MrBayesPrint ("\n");
5006	}
5007
5008	}
5009
5010
5011
5012
5013
5014	/*---------------------------------------------------------------------------------
5015	\|
5016	\| PrintSquareIntegerMatrix
5017	\|
5018	\| Prints a square matrix of integers.
5019	\|
5020	---------------------------------------------------------------------------------*/
5021	void PrintSquareIntegerMatrix (int dim, int **matrix)
5022
5023	{
5024
5025	int i, j;
5026
5027	for (i=0; i<dim; i++)
5028	{
5029	for(j=0; j<dim; j++)
5030	MrBayesPrint ("%d ", matrix[i][j]);
5031	MrBayesPrint ("\n");
5032	}
5033
5034	}
5035
5036
5037
5038
5039
5040	/*---------------------------------------------------------------------------------
5041	\|
5042	\| ProductOfRealAndComplex
5043	\|
5044	\| Returns the complex product of a real and complex number.
5045	\|
5046	---------------------------------------------------------------------------------*/
5047	complex ProductOfRealAndComplex (MrBFlt a, complex b)
5048
5049	{
5050
5051	complex c;
5052
5053	c.re = a * b.re;
5054	c.im = a * b.im;
5055
5056	return (c);
5057
5058	}
5059
5060
5061
5062
5063
5064	/*---------------------------------------------------------------------------------
5065	\|
5066	\| PsiExp: Returns psi (also called digamma) exponentiated
5067	\| Algorithm from http://lib.stat.cmu.edu/apstat/103
5068	\|
5069	---------------------------------------------------------------------------------*/
5070	MrBFlt PsiExp (MrBFlt alpha)
5071
5072	{
5073	MrBFlt digamma, y, r, s, c, s3, s4, s5, d1;
5074
5075	s = 1.0e-05;
5076	c = 8.5;
5077	s3 = 8.333333333333333333333333e-02;
5078	s4 = 8.333333333333333333333333e-03;
5079	s5 = 3.968253968e-03;
5080	d1 = -0.577215664901532860606512; /* negative of Euler's constant */
5081
5082	digamma = 0.0;
5083	y = alpha;
5084	if (y <= 0.0)
5085	return (0.0);
5086
5087	if (y <= s)
5088	{
5089	digamma = d1 - 1.0 / y;
5090	return (exp (digamma));
5091	}
5092
5093	while (y < c)
5094	{
5095	digamma -= 1.0 / y;
5096	y += 1.0;
5097	}
5098
5099	r = 1.0 / y;
5100	digamma += (log (y) - 0.5 * r);
5101	r *= r;
5102	digamma -= r * (s3 - r * (s4 - r * s5));
5103
5104	return (exp (digamma));
5105
5106	}
5107
5108
5109
5110
5111
5112	/*---------------------------------------------------------------------------------
5113	\|
5114	\| PsiGammaLnProb: Calculates the log probability of a PsiGamma distributed
5115	\| variable
5116	\|
5117	---------------------------------------------------------------------------------*/
5118	MrBFlt PsiGammaLnProb (MrBFlt alpha, MrBFlt value)
5119	{
5120	MrBFlt beta, lnProb;
5121
5122	beta = PsiExp (alpha);
5123
5124	lnProb = alpha * log (beta) - LnGamma (alpha) + (alpha - 1.0) * log (value) - beta * value;
5125
5126	return lnProb;
5127	}
5128
5129
5130
5131
5132
5133	/*---------------------------------------------------------------------------------
5134	\|
5135	\| PsiGammaLnRatio: Calculates the log prob ratio of two PsiGamma distributed
5136	\| variables
5137	\|
5138	---------------------------------------------------------------------------------*/
5139	MrBFlt PsiGammaLnRatio (MrBFlt alpha, MrBFlt numerator, MrBFlt denominator)
5140
5141	{
5142	MrBFlt beta, lnRatio;
5143
5144	beta = PsiExp (alpha);
5145
5146	lnRatio = (alpha - 1.0) * (log (numerator) - log (denominator)) - beta * (numerator - denominator);
5147
5148	return (lnRatio);
5149	}
5150
5151
5152
5153
5154
5155	/*---------------------------------------------------------------------------------
5156	\|
5157	\| PsiGammaRandomVariable: Returns a random draw from the PsiGamma
5158	\|
5159	---------------------------------------------------------------------------------*/
5160	MrBFlt PsiGammaRandomVariable (MrBFlt alpha, SafeLong *seed)
5161	{
5162	return GammaRandomVariable (alpha, PsiExp(alpha), seed);
5163	}
5164
5165
5166
5167
5168
5169	/*---------------------------------------------------------------------------------
5170	\|
5171	\| QuantileGamma
5172	\|
5173	---------------------------------------------------------------------------------*/
5174	MrBFlt QuantileGamma (MrBFlt x, MrBFlt alfa, MrBFlt beta)
5175
5176	{
5177
5178	MrBFlt lnga1, quantile;
5179
5180	lnga1 = LnGamma(alfa + 1.0);
5181	quantile = POINTGAMMA(x, alfa, beta);
5182
5183	return (quantile);
5184
5185	}
5186
5187
5188
5189
5190
5191	/*---------------------------------------------------------------------------------
5192	\|
5193	\| RandomNumber
5194	\|
5195	\| This pseudorandom number generator is described in:
5196	\| Park, S. K. and K. W. Miller. 1988. Random number generators: good
5197	\| ones are hard to find. Communications of the ACM, 31(10):1192-1201.
5198	\|
5199	---------------------------------------------------------------------------------*/
5200	MrBFlt RandomNumber (SafeLong *seed)
5201
5202	{
5203	SafeLong lo, hi, test;
5204
5205	hi = (*seed) / 127773;
5206	lo = (*seed) % 127773;
5207	test = 16807 * lo - 2836 * hi;
5208	if (test > 0)
5209	*seed = test;
5210	else
5211	*seed = test + 2147483647;
5212	return ((MrBFlt)(*seed) / (MrBFlt)2147483647);
5213
5214	}
5215
5216
5217
5218
5219
5220	/*---------------------------------------------------------------------------------
5221	\|
5222	\| RndGamma
5223	\|
5224	---------------------------------------------------------------------------------*/
5225	MrBFlt RndGamma (MrBFlt s, SafeLong *seed)
5226
5227	{
5228
5229	MrBFlt r=0.0;
5230
5231	if (s <= 0.0)
5232	puts ("Gamma parameter less than zero\n");
5233
5234	else if (s < 1.0)
5235	r = RndGamma1 (s, seed);
5236	else if (s > 1.0)
5237	r = RndGamma2 (s, seed);
5238	else /* 0-log() == -1 * log(), but =- looks confusing */
5239	r -= log(RandomNumber(seed));
5240
5241	return (r);
5242
5243	}
5244
5245
5246
5247
5248
5249	/*---------------------------------------------------------------------------------
5250	\|
5251	\| RndGamma1
5252	\|
5253	---------------------------------------------------------------------------------*/
5254	MrBFlt RndGamma1 (MrBFlt s, SafeLong *seed)
5255
5256	{
5257
5258	MrBFlt r, x=0.0, small=1e-37, w;
5259	static MrBFlt a, p, uf, ss=10.0, d;
5260
5261	if (fabs(s-ss)>ETA) /* s != ss */
5262	{
5263	a = 1.0 - s;
5264	p = a / (a + s * exp(-a));
5265	uf = p * pow(small / a, s);
5266	d = a * log(a);
5267	ss = s;
5268	}
5269	for (;;)
5270	{
5271	r = RandomNumber (seed);
5272	if (r > p)
5273	x = a - log((1.0 - r) / (1.0 - p)), w = a * log(x) - d;
5274	else if (r>uf)
5275	x = a * pow(r / p, 1.0 / s), w = x;
5276	else
5277	return (0.0);
5278	r = RandomNumber (seed);
5279	if (1.0 - r <= w && r > 0.0)
5280	if (r*(w + 1.0) >= 1.0 \|\| -log(r) <= w)
5281	continue;
5282	break;
5283	}
5284
5285	return (x);
5286
5287	}
5288
5289
5290
5291
5292
5293	/*---------------------------------------------------------------------------------
5294	\|
5295	\| RndGamma2
5296	\|
5297	---------------------------------------------------------------------------------*/
5298	MrBFlt RndGamma2 (MrBFlt s, SafeLong *seed)
5299
5300	{
5301
5302	MrBFlt r , d, f, g, x;
5303	static MrBFlt b, h, ss=0.0;
5304
5305	if (fabs(s-ss)>ETA) /* s != ss */
5306	{
5307	b = s - 1.0;
5308	h = sqrt(3.0 * s - 0.75);
5309	ss = s;
5310	}
5311	for (;;)
5312	{
5313	r = RandomNumber (seed);
5314	g = r - r * r;
5315	f = (r - 0.5) * h / sqrt(g);
5316	x = b + f;
5317	if (x <= 0.0)
5318	continue;
5319	r = RandomNumber (seed);
5320	d = 64 * r * r * g * g * g;
5321	if (d * x < x - 2.0 * f * f \|\| log(d) < 2.0 * (b * log(x / b) - f))
5322	break;
5323	}
5324
5325	return (x);
5326
5327	}
5328
5329
5330
5331
5332
5333	/*---------------------------------------------------------------------------------
5334	\|
5335	\| SetQvalue
5336	\|
5337	\| The Pade method for calculating the matrix exponential, tMat = e^{qMat * v},
5338	\| has an error, e(p,q), that can be controlled by setting p and q to appropriate
5339	\| values. The error is:
5340	\|
5341	\| e(p,q) = 2^(3-(p+q)) * ((p!q!) / (p+q)! (p+q+1)!)
5342	\|
5343	\| Setting p = q will minimize the error for a given amount of work. This function
5344	\| assumes that p = q. The function takes in as a parameter the desired tolerance
5345	\| for the accuracy of the matrix exponentiation, and returns qV = p = q, that
5346	\| will achieve the tolerance. The Pade approximation method is described in:
5347	\|
5348	\| Golub, G. H., and C. F. Van Loan. 1996. Matrix Computations, Third Edition.
5349	\| The Johns Hopkins University Press, Baltimore, Maryland.
5350	\|
5351	\| The function is called from TiProbsUsingPadeApprox.
5352	\|
5353	---------------------------------------------------------------------------------*/
5354	int SetQvalue (MrBFlt tol)
5355
5356	{
5357
5358	int qV;
5359	MrBFlt x;
5360
5361	x = pow(2.0, 3.0 - (0 + 0)) * Factorial(0) * Factorial (0) / (Factorial(0+0) * Factorial (0+0+1));
5362	qV = 0;
5363	while (x > tol)
5364	{
5365	qV++;
5366	x = pow(2.0, 3.0 - (qV + qV)) * Factorial(qV) * Factorial (qV) / (Factorial(qV+qV) * Factorial (qV+qV+1));
5367	}
5368
5369	return (qV);
5370
5371	}
5372
5373
5374
5375
5376
5377	/*---------------------------------------------------------------------------------
5378	\|
5379	\| SetToIdentity
5380	\|
5381	\| Make a dim X dim identity matrix.
5382	\|
5383	---------------------------------------------------------------------------------*/
5384	void SetToIdentity (int dim, MrBFlt **matrix)
5385
5386	{
5387
5388	int row, col;
5389
5390	for (row=0; row<dim; row++)
5391	for (col=0; col<dim; col++)
5392	matrix[row][col] = (row == col ? 1.0 : 0.0);
5393
5394	}
5395
5396
5397
5398
5399
5400	/*---------------------------------------------------------------------------------
5401	\|
5402	\| Tha
5403	\|
5404	\| Calculate Owen's (1956) T(h,a) function, -inf <= h, a <= inf,
5405	\| where h = h1/h2, a = a1/a2, from the program of:
5406	\|
5407	\| Young, J. C. and C. E. Minder. 1974. Algorithm AS 76. An integral
5408	\| useful in calculating non-central t and bivariate normal
5409	\| probabilities. Appl. Statist., 23:455-457. [Correction: Appl.
5410	\| Statist., 28:113 (1979). Remarks: Appl. Statist. 27:379 (1978),
5411	\| 28: 113 (1979), 34:100-101 (1985), 38:580-582 (1988)]
5412	\|
5413	\| See also:
5414	\|
5415	\| Johnson, N. L. and S. Kotz. 1972. Distributions in statistics:
5416	\| multivariate distributions. Wiley and Sons. New York. pp. 93-100.
5417	\|
5418	---------------------------------------------------------------------------------*/
5419	MrBFlt Tha (MrBFlt h1, MrBFlt h2, MrBFlt a1, MrBFlt a2)
5420
5421	{
5422
5423	int ng = 5, i;
5424	MrBFlt U[] = {0.0744372, 0.2166977, 0.3397048, 0.4325317, 0.4869533},
5425	R[] = {0.1477621, 0.1346334, 0.1095432, 0.0747257, 0.0333357},
5426	pai2 = 6.283185307, tv1 = 1e-35, tv2 = 15.0, tv3 = 15.0, tv4 = 1e-5,
5427	a, h, rt, t, x1, x2, r1, r2, s, k, sign = 1.0;
5428
5429	if (fabs(h2) < tv1)
5430	return (0.0);
5431	h = h1 / h2;
5432	if (fabs(a2) < tv1)
5433	{
5434	t = CdfNormal(h);
5435	if (h >= 0.0)
5436	t = (1.0 - t) / 2.0;
5437	else
5438	t /= 2.0;
5439	return (t*(a1 >= 0.0 ? 1.0 : -1.0));
5440	}
5441	a = a1 / a2;
5442	if (a < 0.0)
5443	sign = -1.0;
5444	a = fabs(a);
5445	h = fabs(h);
5446	k = h*a;
5447	if (h > tv2 \|\| a < tv1)
5448	return (0.0);
5449	if (h < tv1)
5450	return (atan(a)/pai2*sign);
5451	if (h < 0.3 && a > 7.0) /* (Boys RJ, 1989) */
5452	{
5453	x1 = exp(-k*k/2.0)/k;
5454	x2 = (CdfNormal(k)-0.5)*sqrt(pai2);
5455	t = 0.25 - (x1+x2)/pai2h + ((1.0+2.0/(kk))x1+x2)/(6.0pai2)hh*h;
5456	return (MAX(t,0)*sign);
5457	}
5458	t = -h*h / 2.0;
5459	x2 = a;
5460	s = a*a;
5461	if (log(1.0+s)-t*s >= tv3)
5462	{
5463	x1 = a/2;
5464	s /= 4.0;
5465	for (;;) /* truncation point by Newton iteration */
5466	{
5467	x2 = x1 + (ts+tv3-log(s+1.0)) / (2.0x1*(1.0/(s+1.0)-t));
5468	s = x2*x2;
5469	if (fabs(x2-x1) < tv4)
5470	break;
5471	x1 = x2;
5472	}
5473	}
5474	for (i=0,rt=0; i<ng; i++) /* Gauss quadrature */
5475	{
5476	r1 = 1.0+s*SQUARE(0.5+U[i]);
5477	r2 = 1.0+s*SQUARE(0.5-U[i]);
5478	rt+= R[i](exp(tr1)/r1 + exp(t*r2)/r2);
5479	}
5480
5481	return (MAX(rtx2/pai2,0)sign);
5482
5483	}
5484
5485
5486
5487
5488
5489	/*---------------------------------------------------------------------------------
5490	\|
5491	\| TiProbsUsingEigens
5492	\|
5493	---------------------------------------------------------------------------------*/
5494	void TiProbsUsingEigens (int dim, MrBFlt cijk, MrBFlt eigenVals, MrBFlt v, MrBFlt r, MrBFlt tMat, MrBFlt fMat, MrBFlt **sMat)
5495
5496	{
5497
5498	int i, j, s;
5499	MrBFlt sum, sumF, sumS, *ptr, EigValexp[192];
5500
5501	for (s=0; s<dim; s++)
5502	EigValexp[s] = exp(eigenVals[s] * v * r);
5503
5504	ptr = cijk;
5505	for (i=0; i<dim; i++)
5506	{
5507	for (j=0; j<dim; j++)
5508	{
5509	sum = 0.0;
5510	for(s=0; s<dim; s++)
5511	sum += (ptr++) EigValexp[s];
5512	tMat[i][j] = (sum < 0.0) ? 0.0 : sum;
5513	}
5514	}
5515
5516	# if 0
5517	for (i=0; i<dim; i++)
5518	{
5519	sum = 0.0;
5520	for (j=0; j<dim; j++)
5521	{
5522	sum += tMat[i][j];
5523	}
5524	if (sum > 1.0001 \|\| sum < 0.9999)
5525	{
5526	MrBayesPrint ("%s Warning: Transition probabilities do not sum to 1.0 (%lf)\n", spacer, sum);
5527	}
5528	}
5529	# endif
5530
5531	if (fMat != NULL && sMat != NULL)
5532	{
5533	ptr = cijk;
5534	for (i=0; i<dim; i++)
5535	{
5536	for (j=0; j<dim; j++)
5537	{
5538	sumF = sumS = 0.0;
5539	for(s=0; s<dim; s++)
5540	{
5541	sumF += (ptr ) eigenVals[s] * r * EigValexp[s];
5542	sumS += (ptr++) eigenVals[s] * eigenVals[s] * r * r * EigValexp[s];
5543	}
5544	fMat[i][j] = sumF;
5545	sMat[i][j] = sumS;
5546	}
5547	}
5548	}
5549
5550	}
5551
5552
5553
5554
5555
5556	/*---------------------------------------------------------------------------------
5557	\|
5558	\| TiProbsUsingPadeApprox
5559	\|
5560	\| The method approximates the matrix exponential, tMat = e^{qMat * v}, using
5561	\| the Pade approximation method, described in:
5562	\|
5563	\| Golub, G. H., and C. F. Van Loan. 1996. Matrix Computations, Third Edition.
5564	\| The Johns Hopkins University Press, Baltimore, Maryland.
5565	\|
5566	\| The method approximates the matrix exponential with accuracy tol.
5567	\|
5568	---------------------------------------------------------------------------------*/
5569	void TiProbsUsingPadeApprox (int dim, MrBFlt qMat, MrBFlt v, MrBFlt r, MrBFlt tMat, MrBFlt fMat, MrBFlt sMat)
5570
5571	{
5572
5573	int qValue;
5574	MrBFlt **a, tol;
5575
5576	tol = 0.0000001;
5577
5578	a = AllocateSquareDoubleMatrix (dim);
5579
5580	MultiplyMatrixByScalar (dim, qMat, v * r, a);
5581
5582	qValue = SetQvalue (tol);
5583
5584	ComputeMatrixExponential (dim, a, qValue, tMat);
5585
5586	FreeSquareDoubleMatrix (a);
5587
5588	if (fMat != NULL && sMat != NULL)
5589	{
5590	MultiplyMatrices (dim, qMat, tMat, fMat);
5591	MultiplyMatrices (dim, qMat, fMat, sMat);
5592	}
5593
5594	}
5595

Note: See TracBrowser for help on using the repository browser.

Context Navigation

source: branches/alilink/GDE/MrBAYES/mrbayes_3.2.1/mbmath.c

Download in other formats: