root/bdm/itpp_ext.cpp @ 343

Revision 343, 8.8 kB (checked in by smidl, 15 years ago)

fix for M_PI on win32

  • Property svn:eol-style set to native
Line 
1//
2// C++ Implementation: itpp_ext
3//
4// Description:
5//
6//
7// Author: smidl <smidl@utia.cas.cz>, (C) 2008
8//
9// Copyright: See COPYING file that comes with this distribution
10//
11//
12
13#include "itpp_ext.h"
14
15#ifndef M_PI
16#define M_PI        3.14159265358979323846
17#endif
18// from  algebra/lapack.h
19
20extern "C" {  /* QR factorization of a general matrix A  */
21        void dgeqrf_ ( int *m, int *n, double *a, int *lda, double *tau, double *work,
22                       int *lwork, int *info );
23};
24
25namespace itpp {
26        Array<int> to_Arr ( const ivec &indices ) {
27                Array<int> a ( indices.size() );
28                for ( int i = 0; i < a.size(); i++ ) {
29                        a ( i ) = indices ( i );
30                }
31                return a;
32        }
33
34        ivec linspace ( int from, int to ) {
35                int n=to-from+1;
36                int i;
37                it_assert_debug ( n>0,"wrong linspace" );
38                ivec iv ( n ); for ( i=0;i<n;i++ ) iv ( i ) =from+i;
39                return iv;
40        };
41
42        void set_subvector ( vec &ov, const ivec &iv, const vec &v )
43        {
44                it_assert_debug ( ( iv.length() <=v.length() ),
45                                                        "Vec<>::set_subvector(ivec, vec<Num_T>): Indexing out "
46                                                                        "of range of v" );
47                for ( int i = 0; i < iv.length(); i++ ) {
48                        it_assert_debug ( iv ( i ) <ov.length(),
49                                                          "Vec<>::set_subvector(ivec, vec<Num_T>): Indexing out "
50                                                                          "of range of v" );
51                        ov ( iv ( i ) ) = v ( i );
52                }
53        }
54
55        vec get_vec(const vec &v, const ivec &indexlist){
56                int size = indexlist.size();
57                vec temp(size);
58                for (int i = 0; i < size; ++i) {
59                        temp(i) = v._data()[indexlist(i)];
60                }
61                return temp;
62        }
63
64// Gamma
65
66        Gamma_RNG::Gamma_RNG ( double a, double b ) {
67                setup ( a,b );
68        }
69
70
71        bvec operator& ( const bvec &a, const bvec &b ) {
72                it_assert_debug ( b.size() ==a.size(), "operator&(): Vectors of different lengths" );
73
74                bvec temp ( a.size() );
75                for ( int i = 0;i < a.size();i++ ) {
76                        temp ( i ) = a ( i ) & b ( i );
77                }
78                return temp;
79        }
80
81        bvec operator| ( const bvec &a, const bvec &b ) {
82                it_assert_debug ( b.size() !=a.size(), "operator&(): Vectors of different lengths" );
83
84                bvec temp ( a.size() );
85                for ( int i = 0;i < a.size();i++ ) {
86                        temp ( i ) = a ( i ) | b ( i );
87                }
88                return temp;
89        }
90#define log std::log
91#define exp std::exp
92#define sqrt std::sqrt
93#define R_FINITE std::isfinite
94
95        double  Gamma_RNG::sample() {
96                //A copy of rgamma code from the R package!!
97                //
98
99                /* Constants : */
100                const static double sqrt32 = 5.656854;
101                const static double exp_m1 = 0.36787944117144232159;/* exp(-1) = 1/e */
102
103                /* Coefficients q[k] - for q0 = sum(q[k]*a^(-k))
104                 * Coefficients a[k] - for q = q0+(t*t/2)*sum(a[k]*v^k)
105                 * Coefficients e[k] - for exp(q)-1 = sum(e[k]*q^k)
106                 */
107                const static double q1 = 0.04166669;
108                const static double q2 = 0.02083148;
109                const static double q3 = 0.00801191;
110                const static double q4 = 0.00144121;
111                const static double q5 = -7.388e-5;
112                const static double q6 = 2.4511e-4;
113                const static double q7 = 2.424e-4;
114
115                const static double a1 = 0.3333333;
116                const static double a2 = -0.250003;
117                const static double a3 = 0.2000062;
118                const static double a4 = -0.1662921;
119                const static double a5 = 0.1423657;
120                const static double a6 = -0.1367177;
121                const static double a7 = 0.1233795;
122
123                /* State variables [FIXME for threading!] :*/
124                static double aa = 0.;
125                static double aaa = 0.;
126                static double s, s2, d;    /* no. 1 (step 1) */
127                static double q0, b, si, c;/* no. 2 (step 4) */
128
129                double e, p, q, r, t, u, v, w, x, ret_val;
130                double a=alpha;
131                double scale=1.0/beta;
132
133                if ( !R_FINITE ( a ) || !R_FINITE ( scale ) || a < 0.0 || scale <= 0.0 )
134                        {it_error ( "Gamma_RNG wrong parameters" );}
135
136                if ( a < 1. ) { /* GS algorithm for parameters a < 1 */
137                        if ( a == 0 )
138                                return 0.;
139                        e = 1.0 + exp_m1 * a;
140                        for ( ;; ) {  //VS repeat
141                                p = e * unif_rand();
142                                if ( p >= 1.0 ) {
143                                        x = -log ( ( e - p ) / a );
144                                        if ( exp_rand() >= ( 1.0 - a ) * log ( x ) )
145                                                break;
146                                }
147                                else {
148                                        x = exp ( log ( p ) / a );
149                                        if ( exp_rand() >= x )
150                                                break;
151                                }
152                        }
153                        return scale * x;
154                }
155
156                /* --- a >= 1 : GD algorithm --- */
157
158                /* Step 1: Recalculations of s2, s, d if a has changed */
159                if ( a != aa ) {
160                        aa = a;
161                        s2 = a - 0.5;
162                        s = sqrt ( s2 );
163                        d = sqrt32 - s * 12.0;
164                }
165                /* Step 2: t = standard normal deviate,
166                           x = (s,1/2) -normal deviate. */
167
168                /* immediate acceptance (i) */
169                t = norm_rand();
170                x = s + 0.5 * t;
171                ret_val = x * x;
172                if ( t >= 0.0 )
173                        return scale * ret_val;
174
175                /* Step 3: u = 0,1 - uniform sample. squeeze acceptance (s) */
176                u = unif_rand();
177                if ( ( d * u ) <= ( t * t * t ) )
178                        return scale * ret_val;
179
180                /* Step 4: recalculations of q0, b, si, c if necessary */
181
182                if ( a != aaa ) {
183                        aaa = a;
184                        r = 1.0 / a;
185                        q0 = ( ( ( ( ( ( q7 * r + q6 ) * r + q5 ) * r + q4 ) * r + q3 ) * r
186                                 + q2 ) * r + q1 ) * r;
187
188                        /* Approximation depending on size of parameter a */
189                        /* The constants in the expressions for b, si and c */
190                        /* were established by numerical experiments */
191
192                        if ( a <= 3.686 ) {
193                                b = 0.463 + s + 0.178 * s2;
194                                si = 1.235;
195                                c = 0.195 / s - 0.079 + 0.16 * s;
196                        }
197                        else if ( a <= 13.022 ) {
198                                b = 1.654 + 0.0076 * s2;
199                                si = 1.68 / s + 0.275;
200                                c = 0.062 / s + 0.024;
201                        }
202                        else {
203                                b = 1.77;
204                                si = 0.75;
205                                c = 0.1515 / s;
206                        }
207                }
208                /* Step 5: no quotient test if x not positive */
209
210                if ( x > 0.0 ) {
211                        /* Step 6: calculation of v and quotient q */
212                        v = t / ( s + s );
213                        if ( fabs ( v ) <= 0.25 )
214                                q = q0 + 0.5 * t * t * ( ( ( ( ( ( a7 * v + a6 ) * v + a5 ) * v + a4 ) * v
215                                                             + a3 ) * v + a2 ) * v + a1 ) * v;
216                        else
217                                q = q0 - s * t + 0.25 * t * t + ( s2 + s2 ) * log ( 1.0 + v );
218
219
220                        /* Step 7: quotient acceptance (q) */
221                        if ( log ( 1.0 - u ) <= q )
222                                return scale * ret_val;
223                }
224
225                for ( ;; ) { //VS repeat
226                        /* Step 8: e = standard exponential deviate
227                         *      u =  0,1 -uniform deviate
228                         *      t = (b,si)-double exponential (laplace) sample */
229                        e = exp_rand();
230                        u = unif_rand();
231                        u = u + u - 1.0;
232                        if ( u < 0.0 )
233                                t = b - si * e;
234                        else
235                                t = b + si * e;
236                        /* Step  9:  rejection if t < tau(1) = -0.71874483771719 */
237                        if ( t >= -0.71874483771719 ) {
238                                /* Step 10:      calculation of v and quotient q */
239                                v = t / ( s + s );
240                                if ( fabs ( v ) <= 0.25 )
241                                        q = q0 + 0.5 * t * t *
242                                            ( ( ( ( ( ( a7 * v + a6 ) * v + a5 ) * v + a4 ) * v + a3 ) * v
243                                                + a2 ) * v + a1 ) * v;
244                                else
245                                        q = q0 - s * t + 0.25 * t * t + ( s2 + s2 ) * log ( 1.0 + v );
246                                /* Step 11:      hat acceptance (h) */
247                                /* (if q not positive go to step 8) */
248                                if ( q > 0.0 ) {
249                                        // TODO: w = expm1(q);
250                                        w = exp ( q )-1;
251                                        /*  ^^^^^ original code had approximation with rel.err < 2e-7 */
252                                        /* if t is rejected sample again at step 8 */
253                                        if ( ( c * fabs ( u ) ) <= ( w * exp ( e - 0.5 * t * t ) ) )
254                                                break;
255                                }
256                        }
257                } /* repeat .. until  `t' is accepted */
258                x = s + 0.5 * t;
259                return scale * x * x;
260        }
261
262
263        bool qr ( const mat &A, mat &R ) {
264                int info;
265                int m = A.rows();
266                int n = A.cols();
267                int lwork = n;
268                int k = std::min ( m, n );
269                vec tau ( k );
270                vec work ( lwork );
271
272                R = A;
273
274                // perform workspace query for optimum lwork value
275                int lwork_tmp = -1;
276                dgeqrf_ ( &m, &n, R._data(), &m, tau._data(), work._data(), &lwork_tmp,
277                          &info );
278                if ( info == 0 ) {
279                        lwork = static_cast<int> ( work ( 0 ) );
280                        work.set_size ( lwork, false );
281                }
282                dgeqrf_ ( &m, &n, R._data(), &m, tau._data(), work._data(), &lwork, &info );
283
284                // construct R
285                for ( int i=0; i<m; i++ )
286                        for ( int j=0; j<std::min ( i,n ); j++ )
287                                R ( i,j ) = 0;
288
289                return ( info==0 );
290        }
291
292
293        std::string num2str(double d){
294                char tmp[20];//that should do
295                sprintf(tmp,"%f",d);
296                return std::string(tmp);
297        };
298        std::string num2str(int i){
299                char tmp[10];//that should do
300                sprintf(tmp,"%d",i);
301                return std::string(tmp);
302        };
303
304// digamma
305// copied from C. Bonds' source
306#include <math.h>
307#define el 0.5772156649015329
308
309double psi(double x) {
310    double s,ps,xa,x2;
311    int n,k;
312    static double a[] = {
313        -0.8333333333333e-01,
314         0.83333333333333333e-02,
315        -0.39682539682539683e-02,
316         0.41666666666666667e-02,
317        -0.75757575757575758e-02,
318         0.21092796092796093e-01,
319        -0.83333333333333333e-01,
320         0.4432598039215686};
321
322    xa = fabs(x);
323    s = 0.0;
324    if ((x == (int)x) && (x <= 0.0)) {
325        ps = 1e308;
326        return ps;
327    }
328    if (xa == (int)xa) {
329        n = xa;
330        for (k=1;k<n;k++) {
331            s += 1.0/k;
332        }
333        ps =  s-el;
334    }
335    else if ((xa+0.5) == ((int)(xa+0.5))) {
336        n = xa-0.5;
337        for (k=1;k<=n;k++) {
338            s += 1.0/(2.0*k-1.0);
339        }
340        ps = 2.0*s-el-1.386294361119891;
341    }
342    else {
343        if (xa < 10.0) {
344            n = 10-(int)xa;
345            for (k=0;k<n;k++) {
346                s += 1.0/(xa+k);
347            }
348            xa += n;
349        }
350        x2 = 1.0/(xa*xa);
351        ps = log(xa)-0.5/xa+x2*(((((((a[7]*x2+a[6])*x2+a[5])*x2+
352            a[4])*x2+a[3])*x2+a[2])*x2+a[1])*x2+a[0]);
353        ps -= s;
354    }
355    if (x < 0.0)
356        ps = ps - M_PI*std::cos(M_PI*x)/std::sin(M_PI*x)-1.0/x;
357        return ps;
358}
359
360}
Note: See TracBrowser for help on using the browser.