Update to 2.0.0 tree from current Fremantle build
[opencv] / src / cxcore / cxrand.cpp
1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 //  By downloading, copying, installing or using the software you agree to this license.
6 //  If you do not agree to this license, do not download, install,
7 //  copy or use the software.
8 //
9 //
10 //                           License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 //   * Redistribution's of source code must retain the above copyright notice,
21 //     this list of conditions and the following disclaimer.
22 //
23 //   * Redistribution's in binary form must reproduce the above copyright notice,
24 //     this list of conditions and the following disclaimer in the documentation
25 //     and/or other materials provided with the distribution.
26 //
27 //   * The name of the copyright holders may not be used to endorse or promote products
28 //     derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 /* ////////////////////////////////////////////////////////////////////
44 //
45 //  Filling CvMat/IplImage instances with random numbers
46 //
47 // */
48
49 #include "_cxcore.h"
50
51 namespace cv
52 {
53
54 ///////////////////////////// Functions Declaration //////////////////////////////////////
55
56 /*
57    Multiply-with-carry generator is used here:
58    temp = ( A*X(n) + carry )
59    X(n+1) = temp mod (2^32)
60    carry = temp / (2^32)
61 */
62
63 #define  RNG_NEXT(x)    ((uint64)(unsigned)(x)*RNG::A + ((x) >> 32))
64
65 /***************************************************************************************\
66 *                           Pseudo-Random Number Generators (PRNGs)                     *
67 \***************************************************************************************/
68
69 template<typename T> static void
70 RandBits_( Mat& _arr, uint64* state, const void* _param )
71 {
72     uint64 temp = *state;
73     const int* param = (const int*)_param;
74     int small_flag = (param[12]|param[13]|param[14]|param[15]) <= 255;
75     Size size = getContinuousSize(_arr,_arr.channels());
76
77     for( int y = 0; y < size.height; y++ )
78     {
79         T* arr = (T*)(_arr.data + _arr.step*y);
80         int i, k = 3;
81         const int* p = param;
82
83         if( !small_flag )
84         {
85             for( i = 0; i <= size.width - 4; i += 4 )
86             {
87                 int t0, t1;
88
89                 temp = RNG_NEXT(temp);
90                 t0 = ((int)temp & p[i + 12]) + p[i];
91                 temp = RNG_NEXT(temp);
92                 t1 = ((int)temp & p[i + 13]) + p[i+1];
93                 arr[i] = saturate_cast<T>(t0);
94                 arr[i+1] = saturate_cast<T>(t1);
95
96                 temp = RNG_NEXT(temp);
97                 t0 = ((int)temp & p[i + 14]) + p[i+2];
98                 temp = RNG_NEXT(temp);
99                 t1 = ((int)temp & p[i + 15]) + p[i+3];
100                 arr[i+2] = saturate_cast<T>(t0);
101                 arr[i+3] = saturate_cast<T>(t1);
102
103                 if( !--k )
104                 {
105                     k = 3;
106                     p -= 12;
107                 }
108             }
109         }
110         else
111         {
112             for( i = 0; i <= size.width - 4; i += 4 )
113             {
114                 int t0, t1, t;
115
116                 temp = RNG_NEXT(temp);
117                 t = (int)temp;
118                 t0 = (t & p[i + 12]) + p[i];
119                 t1 = ((t >> 8) & p[i + 13]) + p[i+1];
120                 arr[i] = saturate_cast<T>(t0);
121                 arr[i+1] = saturate_cast<T>(t1);
122
123                 t0 = ((t >> 16) & p[i + 14]) + p[i + 2];
124                 t1 = ((t >> 24) & p[i + 15]) + p[i + 3];
125                 arr[i+2] = saturate_cast<T>(t0);
126                 arr[i+3] = saturate_cast<T>(t1);
127
128                 if( !--k )
129                 {
130                     k = 3;
131                     p -= 12;
132                 }
133             }
134         }
135
136         for( ; i < size.width; i++ )
137         {
138             unsigned t0;
139             temp = RNG_NEXT(temp);
140
141             t0 = ((int)temp & p[i + 12]) + p[i];
142             arr[i] = saturate_cast<T>(t0);
143         }
144     }
145
146     *state = temp;
147 }
148
149
150 template<typename T, typename PT> static void
151 Randi_( Mat& _arr, uint64* state, const void* _param )
152 {
153     uint64 temp = *state;
154     const PT* param = (const PT*)_param;
155     Size size = getContinuousSize(_arr,_arr.channels());
156
157     for( int y = 0; y < size.height; y++ )
158     {
159         T* arr = (T*)(_arr.data + _arr.step*y);
160         int i, k = 3;
161         const PT* p = param;
162
163         for( i = 0; i <= size.width - 4; i += 4 )
164         {
165             PT f0, f1;
166             temp = RNG_NEXT(temp);
167             f0 = (int)temp * p[i+12] + p[i];
168             temp = RNG_NEXT(temp);
169             f1 = (int)temp * p[i+13] + p[i+1];
170             arr[i] = saturate_cast<T>(cvFloor(f0));
171             arr[i+1] = saturate_cast<T>(cvFloor(f1));
172
173             temp = RNG_NEXT(temp);
174             f0 = (int)temp * p[i+14] + p[i+2];
175             temp = RNG_NEXT(temp);
176             f1 = (int)temp * p[i+15] + p[i+3];
177             arr[i+2] = saturate_cast<T>(cvFloor(f0));
178             arr[i+3] = saturate_cast<T>(cvFloor(f1));
179
180             if( !--k )
181             {
182                 k = 3;
183                 p -= 12;
184             }
185         }
186
187         for( ; i < size.width; i++ )
188         {
189             temp = RNG_NEXT(temp);
190             arr[i] = saturate_cast<T>(cvFloor((int)temp * p[i + 12] + p[i]));
191         }
192     }
193
194     *state = temp;
195 }
196
197
198 static void Randf_( Mat& _arr, uint64* state, const void* _param )
199 {
200     uint64 temp = *state;
201     const float* param = (const float*)_param;
202     Size size = getContinuousSize(_arr,_arr.channels());
203
204     for( int y = 0; y < size.height; y++ )
205     {
206         float* arr = (float*)(_arr.data + _arr.step*y);
207         int i, k = 3;
208         const float* p = param;
209         for( i = 0; i <= size.width - 4; i += 4 )
210         {
211             float f0, f1;
212
213             temp = RNG_NEXT(temp);
214             f0 = (int)temp*p[i+12] + p[i];
215             temp = RNG_NEXT(temp);
216             f1 = (int)temp*p[i+13] + p[i+1];
217             arr[i] = f0; arr[i+1] = f1;
218
219             temp = RNG_NEXT(temp);
220             f0 = (int)temp*p[i+14] + p[i+2];
221             temp = RNG_NEXT(temp);
222             f1 = (int)temp*p[i+15] + p[i+3];
223             arr[i+2] = f0; arr[i+3] = f1;
224
225             if( !--k )
226             {
227                 k = 3;
228                 p -= 12;
229             }
230         }
231
232         for( ; i < size.width; i++ )
233         {
234             temp = RNG_NEXT(temp);
235             arr[i] = (int)temp*p[i+12] + p[i];
236         }
237     }
238
239     *state = temp;
240 }
241
242
243 static void
244 Randd_( Mat& _arr, uint64* state, const void* _param )
245 {
246     uint64 temp = *state;
247     const double* param = (const double*)_param;
248     Size size = getContinuousSize(_arr,_arr.channels());
249     int64 v = 0;
250
251     for( int y = 0; y < size.height; y++ )
252     {
253         double* arr = (double*)(_arr.data + _arr.step*y);
254         int i, k = 3;
255         const double* p = param;
256         
257         for( i = 0; i <= size.width - 4; i += 4 )
258         {
259             double f0, f1;
260
261             temp = RNG_NEXT(temp);
262             v = (temp >> 32)|(temp << 32);
263             f0 = v*p[i+12] + p[i];
264             temp = RNG_NEXT(temp);
265             v = (temp >> 32)|(temp << 32);
266             f1 = v*p[i+12] + p[i];
267             arr[i] = f0; arr[i+1] = f1;
268
269             temp = RNG_NEXT(temp);
270             v = (temp >> 32)|(temp << 32);
271             f0 = v*p[i+12] + p[i];
272             temp = RNG_NEXT(temp);
273             v = (temp >> 32)|(temp << 32);
274             f1 = v*p[i+12] + p[i];
275             arr[i+2] = f0; arr[i+3] = f1;
276
277             if( !--k )
278             {
279                 k = 3;
280                 p -= 12;
281             }
282         }
283
284         for( ; i < size.width; i++ )
285         {
286             temp = RNG_NEXT(temp);
287             v = (temp >> 32)|(temp << 32);
288             arr[i] = v*p[i+12] + p[i];
289         }
290     }
291
292     *state = temp;
293 }
294
295    
296 /*
297    The code below implements the algorithm described in
298    "The Ziggurat Method for Generating Random Variables"
299    by Marsaglia and Tsang, Journal of Statistical Software.
300 */
301 static void
302 Randn_0_1_32f_C1R( float* arr, int len, uint64* state )
303 {
304     const float r = 3.442620f; // The start of the right tail
305     const float rng_flt = 2.3283064365386962890625e-10f; // 2^-32
306     static unsigned kn[127];
307     static float wn[128], fn[128];
308     uint64 temp = *state;
309     static bool initialized=false;
310     int i;
311     
312     if( !initialized )
313     {
314         const double m1 = 2147483648.0;
315         double dn = 3.442619855899, tn = dn, vn = 9.91256303526217e-3;
316         
317         // Set up the tables
318         double q = vn/std::exp(-.5*dn*dn);
319         kn[0] = (unsigned)((dn/q)*m1);
320         kn[1] = 0;
321         
322         wn[0] = (float)(q/m1);
323         wn[127] = (float)(dn/m1);
324         
325         fn[0] = 1.f;
326         fn[127] = (float)std::exp(-.5*dn*dn);
327         
328         for(i=126;i>=1;i--)
329         {
330             dn = std::sqrt(-2.*std::log(vn/dn+std::exp(-.5*dn*dn)));
331             kn[i+1] = (unsigned)((dn/tn)*m1);
332             tn = dn;
333             fn[i] = (float)std::exp(-.5*dn*dn);
334             wn[i] = (float)(dn/m1);
335         }
336         initialized = true;
337     }
338     
339     for( i = 0; i < len; i++ )
340     {
341         float x, y;
342         for(;;)
343         {
344             int hz = (int)temp;
345             temp = RNG_NEXT(temp);
346             int iz = hz & 127;
347             x = hz*wn[iz];
348             if( (unsigned)std::abs(hz) < kn[iz] )
349                 break;
350             if( iz == 0) // iz==0, handles the base strip
351             {
352                 do
353                 {
354                     x = (unsigned)temp*rng_flt;
355                     temp = RNG_NEXT(temp);
356                     y = (unsigned)temp*rng_flt;
357                     temp = RNG_NEXT(temp);
358                     x = (float)(-std::log(x+FLT_MIN)*0.2904764);
359                     y = (float)-std::log(y+FLT_MIN);
360                 }       // .2904764 is 1/r
361                 while( y + y < x*x );
362                 x = hz > 0 ? r + x : -r - x;
363                 break;
364             }
365             // iz > 0, handle the wedges of other strips
366             y = (unsigned)temp*rng_flt;
367             temp = RNG_NEXT(temp);
368             if( fn[iz] + y*(fn[iz - 1] - fn[iz]) < std::exp(-.5*x*x) )
369                 break;
370         }
371         arr[i] = x;
372     }
373     *state = temp;
374 }
375     
376
377 template<typename T, typename PT> static void
378 Randn_( Mat& _arr, uint64* state, const void* _param )
379 {
380     const int RAND_BUF_SIZE = 96;
381     float buffer[RAND_BUF_SIZE];
382     const PT* param = (const PT*)_param;
383     Size size = getContinuousSize(_arr, _arr.channels());
384
385     for( int y = 0; y < size.height; y++ )
386     {
387         T* arr = (T*)(_arr.data + _arr.step*y);
388         int i, j, len = RAND_BUF_SIZE;
389         for( i = 0; i < size.width; i += RAND_BUF_SIZE )
390         {
391             int k = 3;
392             const PT* p = param;
393
394             if( i + len > size.width )
395                 len = size.width - i;
396
397             Randn_0_1_32f_C1R( buffer, len, state );
398
399             for( j = 0; j <= len - 4; j += 4 )
400             {
401                 PT f0, f1;
402
403                 f0 = buffer[j]*p[j+12] + p[j];
404                 f1 = buffer[j+1]*p[j+13] + p[j+1];
405                 arr[i+j] = saturate_cast<T>(f0);
406                 arr[i+j+1] = saturate_cast<T>(f1);
407
408                 f0 = buffer[j+2]*p[j+14] + p[j+2];
409                 f1 = buffer[j+3]*p[j+15] + p[j+3];
410                 arr[i+j+2] = saturate_cast<T>(f0);
411                 arr[i+j+3] = saturate_cast<T>(f1);
412
413                 if( --k == 0 )
414                 {
415                     k = 3;
416                     p -= 12;
417                 }
418             }
419
420             for( ; j < len; j++ )
421                 arr[i+j] = saturate_cast<T>(buffer[j]*p[j+12] + p[j]);
422         }
423     }
424 }
425
426
427 typedef void (*RandFunc)(Mat& dst, uint64* state, const void* param);
428
429 void RNG::fill( Mat& mat, int disttype, const Scalar& param1, const Scalar& param2 )
430 {
431     static RandFunc rngtab[3][8] =
432     {
433         {
434         RandBits_<uchar>, 
435         RandBits_<schar>,
436         RandBits_<ushort>,
437         RandBits_<short>,
438         RandBits_<int>, 0, 0, 0},
439
440         {Randi_<uchar,float>,
441         Randi_<schar,float>,
442         Randi_<ushort,float>,
443         Randi_<short,float>,
444         Randi_<int,float>,
445         Randf_, Randd_, 0},
446
447         {Randn_<uchar,float>,
448         Randn_<schar,float>,
449         Randn_<ushort,float>,
450         Randn_<short,float>,
451         Randn_<int,float>,
452         Randn_<float,float>,
453         Randn_<double,double>, 0}
454     };
455
456     int depth = mat.depth(), channels = mat.channels();
457     double dparam[2][12];
458     float fparam[2][12];
459     int iparam[2][12];
460     void* param = dparam;
461     int i, fast_int_mode = 0;
462     RandFunc func = 0;
463
464     CV_Assert( channels <= 4 );
465
466     if( disttype == UNIFORM )
467     {
468         if( depth <= CV_32S )
469         {
470             for( i = 0, fast_int_mode = 1; i < channels; i++ )
471             {
472                 int t0 = iparam[0][i] = cvCeil(param1.val[i]);
473                 int t1 = iparam[1][i] = cvFloor(param2.val[i]) - t0;
474                 double diff = param1.val[i] - param2.val[i];
475
476                 fast_int_mode &= INT_MIN <= diff && diff <= INT_MAX && (t1 & (t1 - 1)) == 0;
477             }
478         }
479
480         if( fast_int_mode )
481         {
482             for( i = 0; i < channels; i++ )
483                 iparam[1][i]--;
484         
485             for( ; i < 12; i++ )
486             {
487                 int t0 = iparam[0][i - channels];
488                 int t1 = iparam[1][i - channels];
489
490                 iparam[0][i] = t0;
491                 iparam[1][i] = t1;
492             }
493
494             func = rngtab[0][depth];
495             param = iparam;
496         }
497         else
498         {
499             double scale = depth == CV_64F ?
500                 5.4210108624275221700372640043497e-20 : // 2**-64
501                 2.3283064365386962890625e-10;           // 2**-32
502
503             // for each channel i compute such dparam[0][i] & dparam[1][i],
504             // so that a signed 32/64-bit integer X is transformed to
505             // the range [param1.val[i], param2.val[i]) using
506             // dparam[1][i]*X + dparam[0][i]
507             for( i = 0; i < channels; i++ )
508             {
509                 double t0 = param1.val[i];
510                 double t1 = param2.val[i];
511                 dparam[0][i] = (t1 + t0)*0.5;
512                 dparam[1][i] = (t1 - t0)*scale;
513             }
514             
515             func = rngtab[1][depth];
516             param = dparam;
517         }
518     }
519     else if( disttype == CV_RAND_NORMAL )
520     {
521         for( i = 0; i < channels; i++ )
522         {
523             double t0 = param1.val[i];
524             double t1 = param2.val[i];
525
526             dparam[0][i] = t0;
527             dparam[1][i] = t1;
528         }
529
530         func = rngtab[2][depth];
531         param = dparam;
532     }
533     else
534         CV_Error( CV_StsBadArg, "Unknown distribution type" );
535
536     if( !fast_int_mode )
537     {
538         for( i = channels; i < 12; i++ )
539         {
540             double t0 = dparam[0][i - channels];
541             double t1 = dparam[1][i - channels];
542
543             dparam[0][i] = t0;
544             dparam[1][i] = t1;
545         }
546
547         if( depth != CV_64F )
548         {
549             for( i = 0; i < 12; i++ )
550             {
551                 fparam[0][i] = (float)dparam[0][i];
552                 fparam[1][i] = (float)dparam[1][i];
553             }
554             param = fparam;
555         }
556     }
557
558     CV_Assert( func != 0);
559
560     func( mat, &state, param );
561 }
562
563
564 #ifdef WIN32
565 #ifdef WINCE
566 #       define TLS_OUT_OF_INDEXES ((DWORD)0xFFFFFFFF)
567 #endif
568 static DWORD tlsRNGKey = TLS_OUT_OF_INDEXES;
569
570 void deleteThreadRNGData()
571 {
572     if( tlsRNGKey != TLS_OUT_OF_INDEXES )
573         delete (RNG*)TlsGetValue( tlsRNGKey );
574 }
575
576 RNG& theRNG()
577 {
578     if( tlsRNGKey == TLS_OUT_OF_INDEXES )
579     {
580         tlsRNGKey = TlsAlloc();
581         CV_Assert(tlsRNGKey != TLS_OUT_OF_INDEXES);
582     }
583     RNG* rng = (RNG*)TlsGetValue( tlsRNGKey );
584     if( !rng )
585     {
586         rng = new RNG;
587         TlsSetValue( tlsRNGKey, rng );
588     }
589     return *rng;
590 }
591
592 #else
593
594 static pthread_key_t tlsRNGKey = 0;
595
596 static void deleteRNG(void* data)
597 {
598     delete (RNG*)data;
599 }
600
601 RNG& theRNG()
602 {
603     if( !tlsRNGKey )
604     {
605         pthread_key_create(&tlsRNGKey, deleteRNG);
606         CV_Assert(tlsRNGKey != 0);
607     }
608     RNG* rng = (RNG*)pthread_getspecific(tlsRNGKey);
609     if( !rng )
610     {
611         rng = new RNG;
612         pthread_setspecific(tlsRNGKey, rng);
613     }
614     return *rng;
615 }
616
617 #endif
618
619 template<typename T> static void
620 randShuffle_( Mat& _arr, RNG& rng, double iterFactor )
621 {
622     int sz = _arr.rows*_arr.cols, iters = cvRound(iterFactor*sz);
623     if( _arr.isContinuous() )
624     {
625         T* arr = (T*)_arr.data;
626         for( int i = 0; i < iters; i++ )
627         {
628             int j = (unsigned)rng % sz, k = (unsigned)rng % sz;
629             std::swap( arr[j], arr[k] );
630         }
631     }
632     else
633     {
634         uchar* data = _arr.data;
635         size_t step = _arr.step;
636         int cols = _arr.cols;
637         for( int i = 0; i < iters; i++ )
638         {
639             int j1 = (unsigned)rng % sz, k1 = (unsigned)rng % sz;
640             int j0 = j1/cols, k0 = k1/cols;
641             j1 -= j0*cols; k1 -= k0*cols;
642             std::swap( ((T*)(data + step*j0))[j1], ((T*)(data + step*k0))[k1] );
643         }
644     }
645 }
646
647 typedef void (*RandShuffleFunc)( Mat& dst, RNG& rng, double iterFactor );
648
649 void randShuffle( Mat& dst, double iterFactor, RNG* _rng )
650 {
651     RandShuffleFunc tab[] =
652     {
653         0,
654         randShuffle_<uchar>, // 1
655         randShuffle_<ushort>, // 2
656         randShuffle_<Vec<uchar,3> >, // 3
657         randShuffle_<int>, // 4
658         0,
659         randShuffle_<Vec<ushort,3> >, // 6
660         0,
661         randShuffle_<int64>, // 8
662         0, 0, 0,
663         randShuffle_<Vec<int,3> >, // 12
664         0, 0, 0,
665         randShuffle_<Vec<int64,2> >, // 16
666         0, 0, 0, 0, 0, 0, 0,
667         randShuffle_<Vec<int64,3> >, // 24
668         0, 0, 0, 0, 0, 0, 0,
669         randShuffle_<Vec<int64,4> > // 32
670     };
671
672     RNG& rng = _rng ? *_rng : theRNG();
673     CV_Assert( dst.elemSize() <= 32 );
674     RandShuffleFunc func = tab[dst.elemSize()];
675     CV_Assert( func != 0 );
676     func( dst, rng, iterFactor );
677 }
678
679 }
680
681 CV_IMPL void
682 cvRandArr( CvRNG* _rng, CvArr* arr, int disttype, CvScalar param1, CvScalar param2 )
683 {
684     cv::Mat mat = cv::cvarrToMat(arr);
685     // !!! this will only work for current 64-bit MWC RNG !!!
686     cv::RNG& rng = _rng ? (cv::RNG&)*_rng : cv::theRNG();
687     rng.fill(mat, disttype == CV_RAND_NORMAL ?
688         cv::RNG::NORMAL : cv::RNG::UNIFORM, param1, param2 );
689 }
690
691 CV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* _rng, double iter_factor )
692 {
693     cv::Mat dst = cv::cvarrToMat(arr);
694     cv::RNG& rng = _rng ? (cv::RNG&)*_rng : cv::theRNG();
695     cv::randShuffle( dst, iter_factor, &rng );
696 }
697
698 /* End of file. */