1 | /* C implementations of useful functions. |
---|
2 | * Written by Tom Minka (unless otherwise noted). |
---|
3 | */ |
---|
4 | |
---|
5 | #define _USE_MATH_DEFINES 1 |
---|
6 | #include <math.h> |
---|
7 | #include <stdlib.h> |
---|
8 | #include <float.h> |
---|
9 | #include "util.h" |
---|
10 | #include "mex.h" |
---|
11 | |
---|
12 | #ifdef _MSC_VER |
---|
13 | #define finite _finite |
---|
14 | #define isnan _isnan |
---|
15 | #endif |
---|
16 | |
---|
17 | #ifndef M_PI |
---|
18 | #define M_PI 3.14159265358979323846 |
---|
19 | #endif |
---|
20 | |
---|
21 | #ifdef __USE_ISOC99 |
---|
22 | /* INFINITY and NAN are defined by the ISO C99 standard */ |
---|
23 | #else |
---|
24 | double my_infinity(void) { |
---|
25 | double zero = 0; |
---|
26 | return 1.0/zero; |
---|
27 | } |
---|
28 | double my_nan(void) { |
---|
29 | double zero = 0; |
---|
30 | return zero/zero; |
---|
31 | } |
---|
32 | #define INFINITY my_infinity() |
---|
33 | #define NAN my_nan() |
---|
34 | #endif |
---|
35 | |
---|
36 | /* |
---|
37 | * Generates a uniformly distributed r.v. between 0 and 1. |
---|
38 | * Kris Popat 6/85 |
---|
39 | * Ref: Applied Statistics, 1982 vol 31 no 2 pp 188-190 |
---|
40 | * Based on FORTRAN routine by H. Malvar. |
---|
41 | */ |
---|
42 | |
---|
43 | static long ix = 101; |
---|
44 | static long iy = 1001; |
---|
45 | static long iz = 10001; |
---|
46 | |
---|
47 | double Rand(void) |
---|
48 | { |
---|
49 | static float u; |
---|
50 | |
---|
51 | ix = 171*(ix % 177)-2*(ix/177); |
---|
52 | iy = 172*(iy % 176)-2*(iy/176); |
---|
53 | iz = 170*(iz % 178)-2*(iz/178); |
---|
54 | |
---|
55 | if (ix<0) ix = ix + 30269; |
---|
56 | if (iy<0) iy = iy + 30307; |
---|
57 | if (iz<0) iz = iz + 30323; |
---|
58 | |
---|
59 | u = ((float) ix)/30269.0 + |
---|
60 | ((float) iy)/30307.0 + ((float) iz)/30323.0; |
---|
61 | u -= (float)(int)u; |
---|
62 | return(u); |
---|
63 | } |
---|
64 | |
---|
65 | /* Returns a sample from Normal(0,1) |
---|
66 | */ |
---|
67 | double RandN(void) |
---|
68 | { |
---|
69 | static double previous; |
---|
70 | static int usePrevious = 0; |
---|
71 | double x,y,radius; |
---|
72 | if(usePrevious) { |
---|
73 | usePrevious = 0; |
---|
74 | return previous; |
---|
75 | } |
---|
76 | /* Generate a random point inside the unit circle */ |
---|
77 | do { |
---|
78 | x = 2*Rand()-1; |
---|
79 | y = 2*Rand()-1; |
---|
80 | radius = (x*x)+(y*y); |
---|
81 | } while((radius >= 1.0) || (radius == 0.0)); |
---|
82 | /* Box-Muller formula */ |
---|
83 | radius = sqrt(-2*log(radius)/radius); |
---|
84 | x *= radius; |
---|
85 | y *= radius; |
---|
86 | previous = y; |
---|
87 | usePrevious = 1; |
---|
88 | return x; |
---|
89 | } |
---|
90 | |
---|
91 | /* Returns a sample from Gamma(a, 1). |
---|
92 | * For Gamma(a,b), scale the result by b. |
---|
93 | */ |
---|
94 | double GammaRand(double a) |
---|
95 | { |
---|
96 | /* Algorithm: |
---|
97 | * G. Marsaglia and W.W. Tsang, A simple method for generating gamma |
---|
98 | * variables, ACM Transactions on Mathematical Software, Vol. 26, No. 3, |
---|
99 | * Pages 363-372, September, 2000. |
---|
100 | * http://portal.acm.org/citation.cfm?id=358414 |
---|
101 | */ |
---|
102 | double boost, d, c, v; |
---|
103 | if(a < 1) { |
---|
104 | /* boost using Marsaglia's (1961) method: gam(a) = gam(a+1)*U^(1/a) */ |
---|
105 | boost = exp(log(Rand())/a); |
---|
106 | a++; |
---|
107 | } |
---|
108 | else boost = 1; |
---|
109 | d = a-1.0/3; c = 1.0/sqrt(9*d); |
---|
110 | while(1) { |
---|
111 | double x,u; |
---|
112 | do { |
---|
113 | x = RandN(); |
---|
114 | v = 1+c*x; |
---|
115 | } while(v <= 0); |
---|
116 | v = v*v*v; |
---|
117 | x = x*x; |
---|
118 | u = Rand(); |
---|
119 | if((u < 1-.0331*x*x) || |
---|
120 | (log(u) < 0.5*x + d*(1-v+log(v)))) break; |
---|
121 | } |
---|
122 | return( boost*d*v ); |
---|
123 | } |
---|
124 | |
---|
125 | /* Returns a sample from Beta(a,b) */ |
---|
126 | double BetaRand(double a, double b) |
---|
127 | { |
---|
128 | double g = GammaRand(a); |
---|
129 | return g/(g + GammaRand(b)); |
---|
130 | } |
---|
131 | |
---|
132 | /* Very fast binomial sampler. |
---|
133 | * Returns the number of successes out of n trials, with success probability p. |
---|
134 | */ |
---|
135 | int BinoRand(double p, int n) |
---|
136 | { |
---|
137 | int r = 0; |
---|
138 | if(isnan(p)) return 0; |
---|
139 | if(p < DBL_EPSILON) return 0; |
---|
140 | if(p >= 1-DBL_EPSILON) return n; |
---|
141 | if((p > 0.5) && (n < 15)) { |
---|
142 | /* Coin flip method. This takes O(n) time. */ |
---|
143 | int i; |
---|
144 | for(i=0;i<n;i++) { |
---|
145 | if(Rand() < p) r++; |
---|
146 | } |
---|
147 | return r; |
---|
148 | } |
---|
149 | if(n*p < 10) { |
---|
150 | /* Waiting time method. This takes O(np) time. */ |
---|
151 | double q = -log(1-p), e = -log(Rand()), s; |
---|
152 | r = n; |
---|
153 | for(s = e/r; s <= q; s += e/r) { |
---|
154 | r--; |
---|
155 | if(r == 0) break; |
---|
156 | e = -log(Rand()); |
---|
157 | } |
---|
158 | r = n-r; |
---|
159 | return r; |
---|
160 | } |
---|
161 | if (1) { |
---|
162 | /* Recursive method. This makes O(log(log(n))) recursive calls. */ |
---|
163 | int i = floor(p*(n+1)); |
---|
164 | double b = BetaRand(i, n+1-i); |
---|
165 | if(b <= p) r = i + BinoRand((p-b)/(1-b), n-i); |
---|
166 | else r = i - 1 - BinoRand((b-p)/b, i-1); |
---|
167 | return r; |
---|
168 | } |
---|
169 | } |
---|
170 | |
---|
171 | /****************************************************************************/ |
---|
172 | |
---|
173 | double logSum(double a, double b) |
---|
174 | { |
---|
175 | /* make a the larger number */ |
---|
176 | if(a < b) { |
---|
177 | double t = a; a = b; b = t; |
---|
178 | } |
---|
179 | /* log(exp(a) + exp(b)) = a + log(1 + exp(b-a)) */ |
---|
180 | if(!finite(b)) return a; |
---|
181 | return a + log(1 + exp(b-a)); |
---|
182 | } |
---|
183 | |
---|
184 | #define CACHE_SIZE 200 |
---|
185 | |
---|
186 | /* Requires: n >= 0 */ |
---|
187 | double pochhammer(double x, int n) |
---|
188 | { |
---|
189 | static double cache_x = -1; |
---|
190 | static double cache_v[CACHE_SIZE]; |
---|
191 | static int max_cached; |
---|
192 | double result; |
---|
193 | int i; |
---|
194 | /* the maximum n for which we have a cached value */ |
---|
195 | if(n == 0) return 0; |
---|
196 | if(n > CACHE_SIZE) { |
---|
197 | if(x >= 1.e4*n) { |
---|
198 | return log(x) + (n-1)*log(x+n/2); |
---|
199 | } |
---|
200 | return gammaln(x+n) - gammaln(x); |
---|
201 | } |
---|
202 | if(x != cache_x) { |
---|
203 | max_cached = 1; |
---|
204 | cache_v[0] = log(x); |
---|
205 | cache_x = x; |
---|
206 | } |
---|
207 | if(n <= max_cached) return cache_v[n-1]; |
---|
208 | result = cache_v[max_cached-1]; |
---|
209 | x = x + max_cached-1; |
---|
210 | for(i=max_cached;i<n;i++) { |
---|
211 | x = x + 1; |
---|
212 | result += log(x); |
---|
213 | cache_v[i] = result; |
---|
214 | } |
---|
215 | max_cached = n; |
---|
216 | return result; |
---|
217 | } |
---|
218 | |
---|
219 | /* Requires: n >= 0 */ |
---|
220 | double slow_pochhammer(double x, int n) |
---|
221 | { |
---|
222 | double result; |
---|
223 | if(n == 0) return 0; |
---|
224 | if(n <= 20) { |
---|
225 | int i; |
---|
226 | double xi = x; |
---|
227 | /* this assumes x is not too large */ |
---|
228 | result = xi; |
---|
229 | for(i=n-1; i > 0; i--) { |
---|
230 | xi = xi + 1; |
---|
231 | result *= xi; |
---|
232 | } |
---|
233 | result = log(result); |
---|
234 | } |
---|
235 | else if(x >= 1.e4*n) { |
---|
236 | result = log(x) + (n-1)*log(x+n/2); |
---|
237 | } |
---|
238 | else result = gammaln(x+n) - gammaln(x); |
---|
239 | return result; |
---|
240 | } |
---|
241 | |
---|
242 | double di_pochhammer(double x, int n) |
---|
243 | { |
---|
244 | static double cache_x = -1; |
---|
245 | static double cache_v[CACHE_SIZE]; |
---|
246 | static int max_cached; |
---|
247 | double result; |
---|
248 | int i; |
---|
249 | /* the maximum n for which we have a cached value */ |
---|
250 | if(n == 0) return 0; |
---|
251 | if(n > CACHE_SIZE) { |
---|
252 | return digamma(x+n) - digamma(x); |
---|
253 | } |
---|
254 | if(x != cache_x) { |
---|
255 | max_cached = 1; |
---|
256 | cache_v[0] = 1/x; |
---|
257 | cache_x = x; |
---|
258 | } |
---|
259 | if(n <= max_cached) return cache_v[n-1]; |
---|
260 | result = cache_v[max_cached-1]; |
---|
261 | x = x + max_cached-1; |
---|
262 | for(i=max_cached;i<n;i++) { |
---|
263 | x = x + 1; |
---|
264 | result += 1/x; |
---|
265 | cache_v[i] = result; |
---|
266 | } |
---|
267 | max_cached = n; |
---|
268 | return result; |
---|
269 | } |
---|
270 | |
---|
271 | double slow_di_pochhammer(double x, int n) |
---|
272 | { |
---|
273 | double result; |
---|
274 | if(n == 0) return 0; |
---|
275 | if(n <= 20) { |
---|
276 | int i; |
---|
277 | double xi = x; |
---|
278 | result = 1/xi; |
---|
279 | for(i=n-1; i > 0; i--) { |
---|
280 | xi = xi + 1; |
---|
281 | result += 1/xi; |
---|
282 | } |
---|
283 | } |
---|
284 | else result = digamma(x+n) - digamma(x); |
---|
285 | return result; |
---|
286 | } |
---|
287 | |
---|
288 | double tri_pochhammer(double x, int n) |
---|
289 | { |
---|
290 | static double cache_x = -1; |
---|
291 | static double cache_v[CACHE_SIZE]; |
---|
292 | static int max_cached; |
---|
293 | double result; |
---|
294 | int i; |
---|
295 | /* the maximum n for which we have a cached value */ |
---|
296 | if(n == 0) return 0; |
---|
297 | if(n > CACHE_SIZE) { |
---|
298 | return trigamma(x+n) - trigamma(x); |
---|
299 | } |
---|
300 | if(x != cache_x) { |
---|
301 | max_cached = 1; |
---|
302 | cache_v[0] = -1/(x*x); |
---|
303 | cache_x = x; |
---|
304 | } |
---|
305 | if(n <= max_cached) return cache_v[n-1]; |
---|
306 | result = cache_v[max_cached-1]; |
---|
307 | x = x + max_cached-1; |
---|
308 | for(i=max_cached;i<n;i++) { |
---|
309 | x = x + 1; |
---|
310 | result -= 1/(x*x); |
---|
311 | cache_v[i] = result; |
---|
312 | } |
---|
313 | max_cached = n; |
---|
314 | return result; |
---|
315 | } |
---|
316 | |
---|
317 | double slow_tri_pochhammer(double x, int n) |
---|
318 | { |
---|
319 | double result; |
---|
320 | if(n == 0) return 0; |
---|
321 | if(n <= 20) { |
---|
322 | result = -1/(x*x); |
---|
323 | n--; |
---|
324 | while(n > 0) { |
---|
325 | x = x + 1; |
---|
326 | result -= 1/(x*x); |
---|
327 | n--; |
---|
328 | } |
---|
329 | return result; |
---|
330 | } |
---|
331 | return trigamma(x+n) - trigamma(x); |
---|
332 | } |
---|
333 | |
---|
334 | /* Logarithm of multivariate Gamma function, defined as |
---|
335 | * Gamma_d(x) = pi^(d*(d-1)/4)*prod_(i=1..d) Gamma(x + (1-i)/2) |
---|
336 | * http://en.wikipedia.org/wiki/Multivariate_gamma_function |
---|
337 | */ |
---|
338 | double gammaln2(double x, double d) |
---|
339 | { |
---|
340 | #define M_lnPI 1.14472988584940 |
---|
341 | double r = d*(d-1)/4*M_lnPI; |
---|
342 | int i; |
---|
343 | for(i=0; i<d; i++) r += gammaln(x + (1-i)/2); |
---|
344 | return r; |
---|
345 | } |
---|
346 | |
---|
347 | /* Logarithm of the gamma function. |
---|
348 | Returns NaN for negative arguments, even though log(gamma(x)) may |
---|
349 | actually be defined. |
---|
350 | */ |
---|
351 | double gammaln(double x) |
---|
352 | { |
---|
353 | #define M_lnSqrt2PI 0.91893853320467274178 |
---|
354 | static double gamma_series[] = { |
---|
355 | 76.18009172947146, |
---|
356 | -86.50532032941677, |
---|
357 | 24.01409824083091, |
---|
358 | -1.231739572450155, |
---|
359 | 0.1208650973866179e-2, |
---|
360 | -0.5395239384953e-5 |
---|
361 | }; |
---|
362 | int i; |
---|
363 | double denom, x1, series; |
---|
364 | if(x < 0) return NAN; |
---|
365 | if(x == 0) return INFINITY; |
---|
366 | if(!finite(x)) return x; |
---|
367 | /* Lanczos method */ |
---|
368 | denom = x+1; |
---|
369 | x1 = x + 5.5; |
---|
370 | series = 1.000000000190015; |
---|
371 | for(i = 0; i < 6; i++) { |
---|
372 | series += gamma_series[i] / denom; |
---|
373 | denom += 1.0; |
---|
374 | } |
---|
375 | return( M_lnSqrt2PI + (x+0.5)*log(x1) - x1 + log(series/x) ); |
---|
376 | } |
---|
377 | |
---|
378 | /* The digamma function is the derivative of gammaln. |
---|
379 | |
---|
380 | Reference: |
---|
381 | J Bernardo, |
---|
382 | Psi ( Digamma ) Function, |
---|
383 | Algorithm AS 103, |
---|
384 | Applied Statistics, |
---|
385 | Volume 25, Number 3, pages 315-317, 1976. |
---|
386 | |
---|
387 | From http://www.psc.edu/~burkardt/src/dirichlet/dirichlet.f |
---|
388 | (with modifications for negative numbers and extra precision) |
---|
389 | */ |
---|
390 | double digamma(double x) |
---|
391 | { |
---|
392 | double neginf = -INFINITY; |
---|
393 | static const double c = 12, |
---|
394 | d1 = -0.57721566490153286, |
---|
395 | d2 = 1.6449340668482264365, /* pi^2/6 */ |
---|
396 | s = 1e-6, |
---|
397 | s3 = 1./12, |
---|
398 | s4 = 1./120, |
---|
399 | s5 = 1./252, |
---|
400 | s6 = 1./240, |
---|
401 | s7 = 1./132, |
---|
402 | s8 = 691/32760, |
---|
403 | s9 = 1/12, |
---|
404 | s10 = 3617/8160; |
---|
405 | double result; |
---|
406 | /* Illegal arguments */ |
---|
407 | if((x == neginf) || isnan(x)) { |
---|
408 | return NAN; |
---|
409 | } |
---|
410 | /* Singularities */ |
---|
411 | if((x <= 0) && (floor(x) == x)) { |
---|
412 | return neginf; |
---|
413 | } |
---|
414 | /* Negative values */ |
---|
415 | /* Use the reflection formula (Jeffrey 11.1.6): |
---|
416 | * digamma(-x) = digamma(x+1) + pi*cot(pi*x) |
---|
417 | * |
---|
418 | * This is related to the identity |
---|
419 | * digamma(-x) = digamma(x+1) - digamma(z) + digamma(1-z) |
---|
420 | * where z is the fractional part of x |
---|
421 | * For example: |
---|
422 | * digamma(-3.1) = 1/3.1 + 1/2.1 + 1/1.1 + 1/0.1 + digamma(1-0.1) |
---|
423 | * = digamma(4.1) - digamma(0.1) + digamma(1-0.1) |
---|
424 | * Then we use |
---|
425 | * digamma(1-z) - digamma(z) = pi*cot(pi*z) |
---|
426 | */ |
---|
427 | if(x < 0) { |
---|
428 | return digamma(1-x) + M_PI/tan(-M_PI*x); |
---|
429 | } |
---|
430 | /* Use Taylor series if argument <= S */ |
---|
431 | if(x <= s) return d1 - 1/x + d2*x; |
---|
432 | /* Reduce to digamma(X + N) where (X + N) >= C */ |
---|
433 | result = 0; |
---|
434 | while(x < c) { |
---|
435 | result -= 1/x; |
---|
436 | x++; |
---|
437 | } |
---|
438 | /* Use de Moivre's expansion if argument >= C */ |
---|
439 | /* This expansion can be computed in Maple via asympt(Psi(x),x) */ |
---|
440 | if(x >= c) { |
---|
441 | double r = 1/x, t; |
---|
442 | result += log(x) - 0.5*r; |
---|
443 | r *= r; |
---|
444 | #if 0 |
---|
445 | result -= r * (s3 - r * (s4 - r * (s5 - r * (s6 - r * s7)))); |
---|
446 | #else |
---|
447 | /* this version for lame compilers */ |
---|
448 | t = (s5 - r * (s6 - r * s7)); |
---|
449 | result -= r * (s3 - r * (s4 - r * t)); |
---|
450 | #endif |
---|
451 | } |
---|
452 | return result; |
---|
453 | } |
---|
454 | |
---|
455 | /* The trigamma function is the derivative of the digamma function. |
---|
456 | |
---|
457 | Reference: |
---|
458 | |
---|
459 | B Schneider, |
---|
460 | Trigamma Function, |
---|
461 | Algorithm AS 121, |
---|
462 | Applied Statistics, |
---|
463 | Volume 27, Number 1, page 97-99, 1978. |
---|
464 | |
---|
465 | From http://www.psc.edu/~burkardt/src/dirichlet/dirichlet.f |
---|
466 | (with modification for negative arguments and extra precision) |
---|
467 | */ |
---|
468 | double trigamma(double x) |
---|
469 | { |
---|
470 | double neginf = -INFINITY, |
---|
471 | small = 1e-4, |
---|
472 | large = 8, |
---|
473 | c = 1.6449340668482264365, /* pi^2/6 = Zeta(2) */ |
---|
474 | c1 = -2.404113806319188570799476, /* -2 Zeta(3) */ |
---|
475 | b2 = 1./6, |
---|
476 | b4 = -1./30, |
---|
477 | b6 = 1./42, |
---|
478 | b8 = -1./30, |
---|
479 | b10 = 5./66; |
---|
480 | double result; |
---|
481 | /* Illegal arguments */ |
---|
482 | if((x == neginf) || isnan(x)) { |
---|
483 | return NAN; |
---|
484 | } |
---|
485 | /* Singularities */ |
---|
486 | if((x <= 0) && (floor(x) == x)) { |
---|
487 | return neginf; |
---|
488 | } |
---|
489 | /* Negative values */ |
---|
490 | /* Use the derivative of the digamma reflection formula: |
---|
491 | * -trigamma(-x) = trigamma(x+1) - (pi*csc(pi*x))^2 |
---|
492 | */ |
---|
493 | if(x < 0) { |
---|
494 | result = M_PI/sin(-M_PI*x); |
---|
495 | return -trigamma(1-x) + result*result; |
---|
496 | } |
---|
497 | /* Use Taylor series if argument <= small */ |
---|
498 | if(x <= small) { |
---|
499 | return 1/(x*x) + c + c1*x; |
---|
500 | } |
---|
501 | result = 0; |
---|
502 | /* Reduce to trigamma(x+n) where ( X + N ) >= B */ |
---|
503 | while(x < large) { |
---|
504 | result += 1/(x*x); |
---|
505 | x++; |
---|
506 | } |
---|
507 | /* Apply asymptotic formula when X >= B */ |
---|
508 | /* This expansion can be computed in Maple via asympt(Psi(1,x),x) */ |
---|
509 | if(x >= large) { |
---|
510 | double r = 1/(x*x), t; |
---|
511 | #if 0 |
---|
512 | result += 0.5*r + (1 + r*(b2 + r*(b4 + r*(b6 + r*(b8 + r*b10)))))/x; |
---|
513 | #else |
---|
514 | t = (b4 + r*(b6 + r*(b8 + r*b10))); |
---|
515 | result += 0.5*r + (1 + r*(b2 + r*t))/x; |
---|
516 | #endif |
---|
517 | } |
---|
518 | return result; |
---|
519 | } |
---|
520 | |
---|
521 | unsigned *ismember_sorted(double *a, unsigned a_len, double *s, unsigned s_len) |
---|
522 | { |
---|
523 | /* returns a vector tf where tf[i] = 1 if a[i] is in s. */ |
---|
524 | unsigned *tf = mxCalloc(a_len,sizeof(unsigned)); |
---|
525 | unsigned i,j=0; |
---|
526 | if(j == s_len) return tf; |
---|
527 | for(i=0;i<a_len;i++) { |
---|
528 | while(s[j] < a[i]) { |
---|
529 | j++; |
---|
530 | if(j == s_len) return tf; |
---|
531 | } |
---|
532 | if(s[j] == a[i]) tf[i] = 1; |
---|
533 | } |
---|
534 | return tf; |
---|
535 | } |
---|
536 | |
---|
537 | |
---|