vcs.maemo.org Git - opencv/blob - src/cxcore/cxmatrix.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 #include "_cxcore.h"
  44
  45 /****************************************************************************************\
  46 *                           [scaled] Identity matrix initialization                      *
  47 \****************************************************************************************/
  48
  49 namespace cv {
  50
  51 Mat::Mat(const IplImage* img, bool copyData)
  52     : flags(0), rows(0), cols(0), step(0), data(0),
  53       refcount(0), datastart(0), dataend(0)
  54 {
  55     CvMat m, dst;
  56     int coi=0;
  57     cvGetMat( img, &m, &coi );
  58
  59     if( copyData )
  60     {
  61         if( coi == 0 )
  62         {
  63             create( m.rows, m.cols, CV_MAT_TYPE(m.type) );
  64             dst = *this;
  65             cvCopy( &m, &dst );
  66         }
  67         else
  68         {
  69             create( m.rows, m.cols, CV_MAT_DEPTH(m.type) );
  70             dst = *this;
  71             CvMat* pdst = &dst;
  72             const int pairs[] = { coi-1, 0 };
  73             cvMixChannels( (const CvArr**)&img, 1, (CvArr**)&pdst, 1, pairs, 1 );
  74         }
  75     }
  76     else
  77     {
  78         /*if( coi != 0 )
  79             CV_Error(CV_BadCOI, "When copyData=false, COI must not be set");*/
  80
  81         *this = Mat(m.rows, m.cols, CV_MAT_TYPE(m.type), m.data.ptr, m.step);
  82         /*if( img->roi )
  83         {
  84             datastart = (uchar*)img->imageData;
  85             dataend = datastart + img->imageSize;
  86         }*/
  87     }
  88 }
  89
  90 Mat cvarrToMat(const CvArr* arr, bool copyData, bool allowND, int coiMode)
  91 {
  92     Mat m;
  93     if( CV_IS_MAT(arr) )
  94         m = Mat((const CvMat*)arr, copyData );
  95     else if( CV_IS_IMAGE(arr) )
  96     {
  97         const IplImage* iplimg = (const IplImage*)arr;
  98         m = Mat(iplimg, copyData );
  99         if( coiMode == 0 && cvGetImageCOI(iplimg) > 0 )
 100             CV_Error(CV_BadCOI, "COI is not supported by the function");
 101     }
 102     else
 103     {
 104         CvMat hdr, *cvmat = cvGetMat( arr, &hdr, 0, allowND ? 1 : 0 );
 105         if( cvmat )
 106             m = Mat(cvmat, copyData);
 107     }
 108     return m;
 109 }
 110
 111 void extractImageCOI(const CvArr* arr, Mat& ch, int coi)
 112 {
 113     Mat mat = cvarrToMat(arr, false, true, 1);
 114     ch.create(mat.size(), mat.depth());
 115     if(coi < 0)
 116         CV_Assert( CV_IS_IMAGE(arr) && (coi = cvGetImageCOI((const IplImage*)arr)-1) >= 0 );
 117     CV_Assert(0 <= coi && coi < mat.channels());
 118     int _pairs[] = { coi, 0 };
 119     mixChannels( &mat, 1, &ch, 1, _pairs, 1 );
 120 }
 121
 122 void insertImageCOI(const Mat& ch, CvArr* arr, int coi)
 123 {
 124     Mat mat = cvarrToMat(arr, false, true, 1);
 125     if(coi < 0)
 126         CV_Assert( CV_IS_IMAGE(arr) && (coi = cvGetImageCOI((const IplImage*)arr)-1) >= 0 );
 127     CV_Assert(ch.size() == mat.size() && ch.depth() == mat.depth() && 0 <= coi && coi < mat.channels());
 128     int _pairs[] = { 0, coi };
 129     mixChannels( &ch, 1, &mat, 1, _pairs, 1 );
 130 }
 131
 132
 133 Mat Mat::reshape(int new_cn, int new_rows) const
 134 {
 135     Mat hdr = *this;
 136
 137     int cn = channels();
 138     if( new_cn == 0 )
 139         new_cn = cn;
 140
 141     int total_width = cols * cn;
 142
 143     if( (new_cn > total_width || total_width % new_cn != 0) && new_rows == 0 )
 144         new_rows = rows * total_width / new_cn;
 145
 146     if( new_rows != 0 && new_rows != rows )
 147     {
 148         int total_size = total_width * rows;
 149         if( !isContinuous() )
 150             CV_Error( CV_BadStep,
 151             "The matrix is not continuous, thus its number of rows can not be changed" );
 152
 153         if( (unsigned)new_rows > (unsigned)total_size )
 154             CV_Error( CV_StsOutOfRange, "Bad new number of rows" );
 155
 156         total_width = total_size / new_rows;
 157
 158         if( total_width * new_rows != total_size )
 159             CV_Error( CV_StsBadArg, "The total number of matrix elements "
 160                                     "is not divisible by the new number of rows" );
 161
 162         hdr.rows = new_rows;
 163         hdr.step = total_width * elemSize1();
 164     }
 165
 166     int new_width = total_width / new_cn;
 167
 168     if( new_width * new_cn != total_width )
 169         CV_Error( CV_BadNumChannels,
 170         "The total width is not divisible by the new number of channels" );
 171
 172     hdr.cols = new_width;
 173     hdr.flags = (hdr.flags & ~CV_MAT_CN_MASK) | ((new_cn-1) << CV_CN_SHIFT);
 174     return hdr;
 175 }
 176
 177
 178 void
 179 setIdentity( Mat& m, const Scalar& s )
 180 {
 181     int i, j, rows = m.rows, cols = m.cols, type = m.type();
 182
 183     if( type == CV_32FC1 )
 184     {
 185         float* data = (float*)m.data;
 186         float val = (float)s[0];
 187         size_t step = m.step/sizeof(data[0]);
 188
 189         for( i = 0; i < rows; i++, data += step )
 190         {
 191             for( j = 0; j < cols; j++ )
 192                 data[j] = 0;
 193             if( i < cols )
 194                 data[i] = val;
 195         }
 196     }
 197     else if( type == CV_64FC1 )
 198     {
 199         double* data = (double*)m.data;
 200         double val = s[0];
 201         size_t step = m.step/sizeof(data[0]);
 202
 203         for( i = 0; i < rows; i++, data += step )
 204         {
 205             for( j = 0; j < cols; j++ )
 206                 data[j] = 0;
 207             if( i < cols )
 208                 data[i] = val;
 209         }
 210     }
 211     else
 212     {
 213         m = Scalar(0);
 214         m.diag() = s;
 215     }
 216 }
 217
 218 Scalar trace( const Mat& m )
 219 {
 220     int i, type = m.type();
 221     int nm = std::min(m.rows, m.cols);
 222
 223     if( type == CV_32FC1 )
 224     {
 225         const float* ptr = (const float*)m.data;
 226         size_t step = m.step/sizeof(ptr[0]) + 1;
 227         double _s = 0;
 228         for( i = 0; i < nm; i++ )
 229             _s += ptr[i*step];
 230         return _s;
 231     }
 232
 233     if( type == CV_64FC1 )
 234     {
 235         const double* ptr = (const double*)m.data;
 236         size_t step = m.step/sizeof(ptr[0]) + 1;
 237         double _s = 0;
 238         for( i = 0; i < nm; i++ )
 239             _s += ptr[i*step];
 240         return _s;
 241     }
 242
 243     return cv::sum(m.diag());
 244 }
 245
 246
 247 /****************************************************************************************\
 248 *                                       transpose                                        *
 249 \****************************************************************************************/
 250
 251 template<typename T> static void
 252 transposeI_( Mat& mat )
 253 {
 254     int rows = mat.rows, cols = mat.cols;
 255     uchar* data = mat.data;
 256     size_t step = mat.step;
 257
 258     for( int i = 0; i < rows; i++ )
 259     {
 260         T* row = (T*)(data + step*i);
 261         uchar* data1 = data + i*sizeof(T);
 262         for( int j = i+1; j < cols; j++ )
 263             std::swap( row[j], *(T*)(data1 + step*j) );
 264     }
 265 }
 266
 267 template<typename T> static void
 268 transpose_( const Mat& src, Mat& dst )
 269 {
 270     int rows = dst.rows, cols = dst.cols;
 271     uchar* data = src.data;
 272     size_t step = src.step;
 273
 274     for( int i = 0; i < rows; i++ )
 275     {
 276         T* row = (T*)(dst.data + dst.step*i);
 277         uchar* data1 = data + i*sizeof(T);
 278         for( int j = 0; j < cols; j++ )
 279             row[j] = *(T*)(data1 + step*j);
 280     }
 281 }
 282
 283 typedef void (*TransposeInplaceFunc)( Mat& mat );
 284 typedef void (*TransposeFunc)( const Mat& src, Mat& dst );
 285
 286 void transpose( const Mat& src, Mat& dst )
 287 {
 288     TransposeInplaceFunc itab[] =
 289     {
 290         0,
 291         transposeI_<uchar>, // 1
 292         transposeI_<ushort>, // 2
 293         transposeI_<Vec<uchar,3> >, // 3
 294         transposeI_<int>, // 4
 295         0,
 296         transposeI_<Vec<ushort,3> >, // 6
 297         0,
 298         transposeI_<int64>, // 8
 299         0, 0, 0,
 300         transposeI_<Vec<int,3> >, // 12
 301         0, 0, 0,
 302         transposeI_<Vec<int64,2> >, // 16
 303         0, 0, 0, 0, 0, 0, 0,
 304         transposeI_<Vec<int64,3> >, // 24
 305         0, 0, 0, 0, 0, 0, 0,
 306         transposeI_<Vec<int64,4> > // 32
 307     };
 308
 309     TransposeFunc tab[] =
 310     {
 311         0,
 312         transpose_<uchar>, // 1
 313         transpose_<ushort>, // 2
 314         transpose_<Vec<uchar,3> >, // 3
 315         transpose_<int>, // 4
 316         0,
 317         transpose_<Vec<ushort,3> >, // 6
 318         0,
 319         transpose_<int64>, // 8
 320         0, 0, 0,
 321         transpose_<Vec<int,3> >, // 12
 322         0, 0, 0,
 323         transpose_<Vec<int64,2> >, // 16
 324         0, 0, 0, 0, 0, 0, 0,
 325         transpose_<Vec<int64,3> >, // 24
 326         0, 0, 0, 0, 0, 0, 0,
 327         transpose_<Vec<int64,4> > // 32
 328     };
 329
 330     size_t esz = src.elemSize();
 331     CV_Assert( esz <= (size_t)32 );
 332
 333     if( dst.data == src.data && dst.cols == dst.rows )
 334     {
 335         TransposeInplaceFunc func = itab[esz];
 336         CV_Assert( func != 0 );
 337         func( dst );
 338     }
 339     else
 340     {
 341         dst.create( src.cols, src.rows, src.type() );
 342         TransposeFunc func = tab[esz];
 343         CV_Assert( func != 0 );
 344         func( src, dst );
 345     }
 346 }
 347
 348
 349 void completeSymm( Mat& matrix, bool LtoR )
 350 {
 351     int i, j, nrows = matrix.rows, type = matrix.type();
 352     int j0 = 0, j1 = nrows;
 353     CV_Assert( matrix.rows == matrix.cols );
 354
 355     if( type == CV_32FC1 || type == CV_32SC1 )
 356     {
 357         int* data = (int*)matrix.data;
 358         size_t step = matrix.step/sizeof(data[0]);
 359         for( i = 0; i < nrows; i++ )
 360         {
 361             if( !LtoR ) j1 = i; else j0 = i+1;
 362             for( j = j0; j < j1; j++ )
 363                 data[i*step + j] = data[j*step + i];
 364         }
 365     }
 366     else if( type == CV_64FC1 )
 367     {
 368         double* data = (double*)matrix.data;
 369         size_t step = matrix.step/sizeof(data[0]);
 370         for( i = 0; i < nrows; i++ )
 371         {
 372             if( !LtoR ) j1 = i; else j0 = i+1;
 373             for( j = j0; j < j1; j++ )
 374                 data[i*step + j] = data[j*step + i];
 375         }
 376     }
 377     else
 378         CV_Error( CV_StsUnsupportedFormat, "" );
 379 }
 380
 381 Mat Mat::cross(const Mat& m) const
 382 {
 383     int t = type(), d = CV_MAT_DEPTH(t);
 384     CV_Assert( size() == m.size() && t == m.type() &&
 385         ((rows == 3 && cols == 1) || (cols*channels() == 3 && rows == 1)));
 386     Mat result(rows, cols, t);
 387
 388     if( d == CV_32F )
 389     {
 390         const float *a = (const float*)data, *b = (const float*)m.data;
 391         float* c = (float*)result.data;
 392         size_t lda = rows > 1 ? step/sizeof(a[0]) : 1;
 393         size_t ldb = rows > 1 ? m.step/sizeof(b[0]) : 1;
 394
 395         c[0] = a[lda] * b[ldb*2] - a[lda*2] * b[ldb];
 396         c[1] = a[lda*2] * b[0] - a[0] * b[ldb*2];
 397         c[2] = a[0] * b[ldb] - a[lda] * b[0];
 398     }
 399     else if( d == CV_64F )
 400     {
 401         const double *a = (const double*)data, *b = (const double*)m.data;
 402         double* c = (double*)result.data;
 403         size_t lda = rows > 1 ? step/sizeof(a[0]) : 1;
 404         size_t ldb = rows > 1 ? m.step/sizeof(b[0]) : 1;
 405
 406         c[0] = a[lda] * b[ldb*2] - a[lda*2] * b[ldb];
 407         c[1] = a[lda*2] * b[0] - a[0] * b[ldb*2];
 408         c[2] = a[0] * b[ldb] - a[lda] * b[0];
 409     }
 410
 411     return result;
 412 }
 413
 414
 415 /****************************************************************************************\
 416 *                                Reduce Mat to vector                                 *
 417 \****************************************************************************************/
 418
 419 template<typename T, typename ST, class Op> static void
 420 reduceR_( const Mat& srcmat, Mat& dstmat )
 421 {
 422     typedef typename Op::rtype WT;
 423     Size size = srcmat.size();
 424     size.width *= srcmat.channels();
 425     AutoBuffer<WT> buffer(size.width);
 426     WT* buf = buffer;
 427     ST* dst = (ST*)dstmat.data;
 428     const T* src = (const T*)srcmat.data;
 429     size_t srcstep = srcmat.step/sizeof(src[0]);
 430     int i;
 431     Op op;
 432
 433     for( i = 0; i < size.width; i++ )
 434         buf[i] = src[i];
 435
 436     for( ; --size.height; )
 437     {
 438         src += srcstep;
 439         for( i = 0; i <= size.width - 4; i += 4 )
 440         {
 441             WT s0, s1;
 442             s0 = op(buf[i], (WT)src[i]);
 443             s1 = op(buf[i+1], (WT)src[i+1]);
 444             buf[i] = s0; buf[i+1] = s1;
 445
 446             s0 = op(buf[i+2], (WT)src[i+2]);
 447             s1 = op(buf[i+3], (WT)src[i+3]);
 448             buf[i+2] = s0; buf[i+3] = s1;
 449         }
 450
 451         for( ; i < size.width; i++ )
 452             buf[i] = op(buf[i], (WT)src[i]);
 453     }
 454
 455     for( i = 0; i < size.width; i++ )
 456         dst[i] = (ST)buf[i];
 457 }
 458
 459
 460 template<typename T, typename ST, class Op> static void
 461 reduceC_( const Mat& srcmat, Mat& dstmat )
 462 {
 463     typedef typename Op::rtype WT;
 464     Size size = srcmat.size();
 465     int i, k, cn = srcmat.channels();
 466     size.width *= cn;
 467     Op op;
 468
 469     for( int y = 0; y < size.height; y++ )
 470     {
 471         const T* src = (const T*)(srcmat.data + srcmat.step*y);
 472         ST* dst = (ST*)(dstmat.data + dstmat.step*y);
 473         if( size.width == cn )
 474             for( k = 0; k < cn; k++ )
 475                 dst[k] = src[k];
 476         else
 477         {
 478             for( k = 0; k < cn; k++ )
 479             {
 480                 WT a0 = src[k], a1 = src[k+cn];
 481                 for( i = 2*cn; i <= size.width - 4*cn; i += 4*cn )
 482                 {
 483                     a0 = op(a0, (WT)src[i+k]);
 484                     a1 = op(a1, (WT)src[i+k+cn]);
 485                     a0 = op(a0, (WT)src[i+k+cn*2]);
 486                     a1 = op(a1, (WT)src[i+k+cn*3]);
 487                 }
 488
 489                 for( ; i < size.width; i += cn )
 490                 {
 491                     a0 = op(a0, (WT)src[i]);
 492                 }
 493                 a0 = op(a0, a1);
 494                 dst[k] = (ST)a0;
 495             }
 496         }
 497     }
 498 }
 499
 500 typedef void (*ReduceFunc)( const Mat& src, Mat& dst );
 501
 502 void reduce(const Mat& src, Mat& dst, int dim, int op, int dtype)
 503 {
 504     int op0 = op;
 505     int stype = src.type(), sdepth = src.depth();
 506     if( dtype < 0 )
 507         dtype = stype;
 508     int ddepth = CV_MAT_DEPTH(dtype);
 509
 510     dst.create(dim == 0 ? 1 : src.rows, dim == 0 ? src.cols : 1, dtype >= 0 ? dtype : stype);
 511     Mat temp = dst;
 512
 513     CV_Assert( op == CV_REDUCE_SUM || op == CV_REDUCE_MAX ||
 514         op == CV_REDUCE_MIN || op == CV_REDUCE_AVG );
 515     CV_Assert( src.channels() == dst.channels() );
 516
 517     if( op == CV_REDUCE_AVG )
 518     {
 519         op = CV_REDUCE_SUM;
 520         if( sdepth < CV_32S && ddepth < CV_32S )
 521             temp.create(dst.rows, dst.cols, CV_32SC(src.channels()));
 522     }
 523
 524     ReduceFunc func = 0;
 525     if( dim == 0 )
 526     {
 527         if( op == CV_REDUCE_SUM )
 528         {
 529             if(sdepth == CV_8U && ddepth == CV_32S)
 530                 func = reduceR_<uchar,int,OpAdd<int> >;
 531             if(sdepth == CV_8U && ddepth == CV_32F)
 532                 func = reduceR_<uchar,float,OpAdd<int> >;
 533             if(sdepth == CV_8U && ddepth == CV_64F)
 534                 func = reduceR_<uchar,double,OpAdd<int> >;
 535             if(sdepth == CV_16U && ddepth == CV_32F)
 536                 func = reduceR_<ushort,float,OpAdd<float> >;
 537             if(sdepth == CV_16U && ddepth == CV_64F)
 538                 func = reduceR_<ushort,double,OpAdd<double> >;
 539             if(sdepth == CV_16S && ddepth == CV_32F)
 540                 func = reduceR_<short,float,OpAdd<float> >;
 541             if(sdepth == CV_16S && ddepth == CV_64F)
 542                 func = reduceR_<short,double,OpAdd<double> >;
 543             if(sdepth == CV_32F && ddepth == CV_32F)
 544                 func = reduceR_<float,float,OpAdd<float> >;
 545             if(sdepth == CV_32F && ddepth == CV_64F)
 546                 func = reduceR_<float,double,OpAdd<double> >;
 547             if(sdepth == CV_64F && ddepth == CV_64F)
 548                 func = reduceR_<double,double,OpAdd<double> >;
 549         }
 550         else if(op == CV_REDUCE_MAX)
 551         {
 552             if(sdepth == CV_8U && ddepth == CV_8U)
 553                 func = reduceR_<uchar, uchar, OpMax<uchar> >;
 554             if(sdepth == CV_32F && ddepth == CV_32F)
 555                 func = reduceR_<float, float, OpMax<float> >;
 556             if(sdepth == CV_64F && ddepth == CV_64F)
 557                 func = reduceR_<double, double, OpMax<double> >;
 558         }
 559         else if(op == CV_REDUCE_MIN)
 560         {
 561             if(sdepth == CV_8U && ddepth == CV_8U)
 562                 func = reduceR_<uchar, uchar, OpMin<uchar> >;
 563             if(sdepth == CV_32F && ddepth == CV_32F)
 564                 func = reduceR_<float, float, OpMin<float> >;
 565             if(sdepth == CV_64F && ddepth == CV_64F)
 566                 func = reduceR_<double, double, OpMin<double> >;
 567         }
 568     }
 569     else
 570     {
 571         if(op == CV_REDUCE_SUM)
 572         {
 573             if(sdepth == CV_8U && ddepth == CV_32S)
 574                 func = reduceC_<uchar,int,OpAdd<int> >;
 575             if(sdepth == CV_8U && ddepth == CV_32F)
 576                 func = reduceC_<uchar,float,OpAdd<int> >;
 577             if(sdepth == CV_8U && ddepth == CV_64F)
 578                 func = reduceC_<uchar,double,OpAdd<int> >;
 579             if(sdepth == CV_16U && ddepth == CV_32F)
 580                 func = reduceC_<ushort,float,OpAdd<float> >;
 581             if(sdepth == CV_16U && ddepth == CV_64F)
 582                 func = reduceC_<ushort,double,OpAdd<double> >;
 583             if(sdepth == CV_16S && ddepth == CV_32F)
 584                 func = reduceC_<short,float,OpAdd<float> >;
 585             if(sdepth == CV_16S && ddepth == CV_64F)
 586                 func = reduceC_<short,double,OpAdd<double> >;
 587             if(sdepth == CV_32F && ddepth == CV_32F)
 588                 func = reduceC_<float,float,OpAdd<float> >;
 589             if(sdepth == CV_32F && ddepth == CV_64F)
 590                 func = reduceC_<float,double,OpAdd<double> >;
 591             if(sdepth == CV_64F && ddepth == CV_64F)
 592                 func = reduceC_<double,double,OpAdd<double> >;
 593         }
 594         else if(op == CV_REDUCE_MAX)
 595         {
 596             if(sdepth == CV_8U && ddepth == CV_8U)
 597                 func = reduceC_<uchar, uchar, OpMax<uchar> >;
 598             if(sdepth == CV_32F && ddepth == CV_32F)
 599                 func = reduceC_<float, float, OpMax<float> >;
 600             if(sdepth == CV_64F && ddepth == CV_64F)
 601                 func = reduceC_<double, double, OpMax<double> >;
 602         }
 603         else if(op == CV_REDUCE_MIN)
 604         {
 605             if(sdepth == CV_8U && ddepth == CV_8U)
 606                 func = reduceC_<uchar, uchar, OpMin<uchar> >;
 607             if(sdepth == CV_32F && ddepth == CV_32F)
 608                 func = reduceC_<float, float, OpMin<float> >;
 609             if(sdepth == CV_64F && ddepth == CV_64F)
 610                 func = reduceC_<double, double, OpMin<double> >;
 611         }
 612     }
 613
 614     if( !func )
 615         CV_Error( CV_StsUnsupportedFormat,
 616         "Unsupported combination of input and output array formats" );
 617
 618     func( src, temp );
 619
 620     if( op0 == CV_REDUCE_AVG )
 621         temp.convertTo(dst, dst.type(), 1./(dim == 0 ? src.rows : src.cols));
 622 }
 623
 624
 625 template<typename T> static void sort_( const Mat& src, Mat& dst, int flags )
 626 {
 627     AutoBuffer<T> buf;
 628     T* bptr;
 629     int i, j, n, len;
 630     bool sortRows = (flags & 1) == CV_SORT_EVERY_ROW;
 631     bool inplace = src.data == dst.data;
 632     bool sortDescending = (flags & CV_SORT_DESCENDING) != 0;
 633
 634     if( sortRows )
 635         n = src.rows, len = src.cols;
 636     else
 637     {
 638         n = src.cols, len = src.rows;
 639         buf.allocate(len);
 640     }
 641     bptr = (T*)buf;
 642
 643     for( i = 0; i < n; i++ )
 644     {
 645         T* ptr = bptr;
 646         if( sortRows )
 647         {
 648             T* dptr = (T*)(dst.data + dst.step*i);
 649             if( !inplace )
 650             {
 651                 const T* sptr = (const T*)(src.data + src.step*i);
 652                 for( j = 0; j < len; j++ )
 653                     dptr[j] = sptr[j];
 654             }
 655             ptr = dptr;
 656         }
 657         else
 658         {
 659             for( j = 0; j < len; j++ )
 660                 ptr[j] = ((const T*)(src.data + src.step*j))[i];
 661         }
 662         std::sort( ptr, ptr + len, LessThan<T>() );
 663         if( sortDescending )
 664             for( j = 0; j < len/2; j++ )
 665                 std::swap(ptr[j], ptr[len-1-j]);
 666         if( !sortRows )
 667             for( j = 0; j < len; j++ )
 668                 ((T*)(dst.data + dst.step*j))[i] = ptr[j];
 669     }
 670 }
 671
 672
 673 template<typename T> static void sortIdx_( const Mat& src, Mat& dst, int flags )
 674 {
 675     AutoBuffer<T> buf;
 676     AutoBuffer<int> ibuf;
 677     T* bptr;
 678     int* _iptr;
 679     int i, j, n, len;
 680     bool sortRows = (flags & 1) == CV_SORT_EVERY_ROW;
 681     bool sortDescending = (flags & CV_SORT_DESCENDING) != 0;
 682
 683     CV_Assert( src.data != dst.data );
 684
 685     if( sortRows )
 686         n = src.rows, len = src.cols;
 687     else
 688     {
 689         n = src.cols, len = src.rows;
 690         buf.allocate(len);
 691         ibuf.allocate(len);
 692     }
 693     bptr = (T*)buf;
 694     _iptr = (int*)ibuf;
 695
 696     for( i = 0; i < n; i++ )
 697     {
 698         T* ptr = bptr;
 699         int* iptr = _iptr;
 700
 701         if( sortRows )
 702         {
 703             ptr = (T*)(src.data + src.step*i);
 704             iptr = (int*)(dst.data + dst.step*i);
 705         }
 706         else
 707         {
 708             for( j = 0; j < len; j++ )
 709                 ptr[j] = ((const T*)(src.data + src.step*j))[i];
 710         }
 711         for( j = 0; j < len; j++ )
 712             iptr[j] = j;
 713         std::sort( iptr, iptr + len, LessThanIdx<T>(ptr) );
 714         if( sortDescending )
 715             for( j = 0; j < len/2; j++ )
 716                 std::swap(iptr[j], iptr[len-1-j]);
 717         if( !sortRows )
 718             for( j = 0; j < len; j++ )
 719                 ((int*)(dst.data + dst.step*j))[i] = iptr[j];
 720     }
 721 }
 722
 723 typedef void (*SortFunc)(const Mat& src, Mat& dst, int flags);
 724
 725 void sort( const Mat& src, Mat& dst, int flags )
 726 {
 727     static SortFunc tab[] =
 728     {
 729         sort_<uchar>, sort_<schar>, sort_<ushort>, sort_<short>,
 730         sort_<int>, sort_<float>, sort_<double>, 0
 731     };
 732     SortFunc func = tab[src.depth()];
 733     CV_Assert( src.channels() == 1 && func != 0 );
 734     dst.create( src.size(), src.type() );
 735     func( src, dst, flags );
 736 }
 737
 738 void sortIdx( const Mat& src, Mat& dst, int flags )
 739 {
 740     static SortFunc tab[] =
 741     {
 742         sortIdx_<uchar>, sortIdx_<schar>, sortIdx_<ushort>, sortIdx_<short>,
 743         sortIdx_<int>, sortIdx_<float>, sortIdx_<double>, 0
 744     };
 745     SortFunc func = tab[src.depth()];
 746     CV_Assert( src.channels() == 1 && func != 0 );
 747     if( dst.data == src.data )
 748         dst.release();
 749     dst.create( src.size(), CV_32S );
 750     func( src, dst, flags );
 751 }
 752
 753 static void generateRandomCenter(const vector<Vec2f>& box, float* center, RNG& rng)
 754 {
 755     size_t j, dims = box.size();
 756     float margin = 1.f/dims;
 757     for( j = 0; j < dims; j++ )
 758         center[j] = ((float)rng*(1.f+margin*2.f)-margin)*(box[j][1] - box[j][0]) + box[j][0];
 759 }
 760
 761
 762 static inline float distance(const float* a, const float* b, int n)
 763 {
 764     int j = 0; float d = 0.f;
 765 #if CV_SSE2
 766     float CV_DECL_ALIGNED(16) buf[4];
 767     __m128 d0 = _mm_setzero_ps(), d1 = _mm_setzero_ps();
 768
 769     for( ; j <= n - 8; j += 8 )
 770     {
 771         __m128 t0 = _mm_sub_ps(_mm_loadu_ps(a + j), _mm_loadu_ps(b + j));
 772         __m128 t1 = _mm_sub_ps(_mm_loadu_ps(a + j + 4), _mm_loadu_ps(b + j + 4));
 773         d0 = _mm_add_ps(d0, _mm_mul_ps(t0, t0));
 774         d1 = _mm_add_ps(d1, _mm_mul_ps(t1, t1));
 775     }
 776     _mm_store_ps(buf, _mm_add_ps(d0, d1));
 777     d = buf[0] + buf[1] + buf[2] + buf[3];
 778 #else
 779     for( ; j <= n - 4; j += 4 )
 780     {
 781         float t0 = a[j] - b[j], t1 = a[j+1] - b[j+1], t2 = a[j+2] - b[j+2], t3 = a[j+3] - b[j+3];
 782         d += t0*t0 + t1*t1 + t2*t2 + t3*t3;
 783     }
 784 #endif
 785     for( ; j < n; j++ )
 786     {
 787         float t = a[j] - b[j];
 788         d += t*t;
 789     }
 790     return d;
 791 }
 792
 793 /*
 794 k-means center initialization using the following algorithm:
 795 Arthur & Vassilvitskii (2007) k-means++: The Advantages of Careful Seeding
 796 */
 797 static void generateCentersPP(const Mat& _data, Mat& _out_centers,
 798                               int K, RNG& rng, int trials)
 799 {
 800     int i, j, k, dims = _data.cols, N = _data.rows;
 801     const float* data = _data.ptr<float>(0);
 802     int step = _data.step/sizeof(data[0]);
 803     vector<int> _centers(K);
 804     int* centers = &_centers[0];
 805     vector<float> _dist(N*3);
 806     float* dist = &_dist[0], *tdist = dist + N, *tdist2 = tdist + N;
 807     double sum0 = 0;
 808
 809     centers[0] = (unsigned)rng % N;
 810
 811     for( i = 0; i < N; i++ )
 812     {
 813         dist[i] = distance(data + step*i, data + step*centers[0], dims);
 814         sum0 += dist[i];
 815     }
 816
 817     for( k = 1; k < K; k++ )
 818     {
 819         double bestSum = DBL_MAX;
 820         int bestCenter = -1;
 821
 822         for( j = 0; j < trials; j++ )
 823         {
 824             double p = (double)rng*sum0, s = 0;
 825             for( i = 0; i < N-1; i++ )
 826                 if( (p -= dist[i]) <= 0 )
 827                     break;
 828             int ci = i;
 829             for( i = 0; i < N; i++ )
 830             {
 831                 tdist2[i] = std::min(distance(data + step*i, data + step*ci, dims), dist[i]);
 832                 s += tdist2[i];
 833             }
 834
 835             if( s < bestSum )
 836             {
 837                 bestSum = s;
 838                 bestCenter = ci;
 839                 std::swap(tdist, tdist2);
 840             }
 841         }
 842         centers[k] = bestCenter;
 843         sum0 = bestSum;
 844         std::swap(dist, tdist);
 845     }
 846
 847     for( k = 0; k < K; k++ )
 848     {
 849         const float* src = data + step*centers[k];
 850         float* dst = _out_centers.ptr<float>(k);
 851         for( j = 0; j < dims; j++ )
 852             dst[j] = src[j];
 853     }
 854 }
 855
 856 double kmeans( const Mat& data, int K, Mat& best_labels,
 857                TermCriteria criteria, int attempts,
 858                int flags, Mat* _centers )
 859 {
 860     const int SPP_TRIALS = 3;
 861     int N = data.rows > 1 ? data.rows : data.cols;
 862     int dims = (data.rows > 1 ? data.cols : 1)*data.channels();
 863     int type = data.depth();
 864
 865     attempts = std::max(attempts, 1);
 866     CV_Assert( type == CV_32F && K > 0 );
 867
 868     Mat _labels;
 869     if( flags & CV_KMEANS_USE_INITIAL_LABELS )
 870     {
 871         CV_Assert( (best_labels.cols == 1 || best_labels.rows == 1) &&
 872                   best_labels.cols*best_labels.rows == N &&
 873                   best_labels.type() == CV_32S &&
 874                   best_labels.isContinuous());
 875         best_labels.copyTo(_labels);
 876     }
 877     else
 878     {
 879         if( !((best_labels.cols == 1 || best_labels.rows == 1) &&
 880              best_labels.cols*best_labels.rows == N &&
 881             best_labels.type() == CV_32S &&
 882             best_labels.isContinuous()))
 883             best_labels.create(N, 1, CV_32S);
 884         _labels.create(best_labels.size(), best_labels.type());
 885     }
 886     int* labels = _labels.ptr<int>();
 887
 888     Mat centers(K, dims, type), old_centers(K, dims, type);
 889     vector<int> counters(K);
 890     vector<Vec2f> _box(dims);
 891     Vec2f* box = &_box[0];
 892
 893     double best_compactness = DBL_MAX, compactness = 0;
 894     RNG& rng = theRNG();
 895     int a, iter, i, j, k;
 896
 897     if( criteria.type & TermCriteria::EPS )
 898         criteria.epsilon = std::max(criteria.epsilon, 0.);
 899     else
 900         criteria.epsilon = FLT_EPSILON;
 901     criteria.epsilon *= criteria.epsilon;
 902
 903     if( criteria.type & TermCriteria::COUNT )
 904         criteria.maxCount = std::min(std::max(criteria.maxCount, 2), 100);
 905     else
 906         criteria.maxCount = 100;
 907
 908     if( K == 1 )
 909     {
 910         attempts = 1;
 911         criteria.maxCount = 2;
 912     }
 913
 914     const float* sample = data.ptr<float>(0);
 915     for( j = 0; j < dims; j++ )
 916         box[j] = Vec2f(sample[j], sample[j]);
 917
 918     for( i = 1; i < N; i++ )
 919     {
 920         sample = data.ptr<float>(i);
 921         for( j = 0; j < dims; j++ )
 922         {
 923             float v = sample[j];
 924             box[j][0] = std::min(box[j][0], v);
 925             box[j][1] = std::max(box[j][1], v);
 926         }
 927     }
 928
 929     for( a = 0; a < attempts; a++ )
 930     {
 931         double max_center_shift = DBL_MAX;
 932         for( iter = 0; iter < criteria.maxCount && max_center_shift > criteria.epsilon; iter++ )
 933         {
 934             swap(centers, old_centers);
 935
 936             if( iter == 0 && (a > 0 || !(flags & KMEANS_USE_INITIAL_LABELS)) )
 937             {
 938                 if( flags & KMEANS_PP_CENTERS )
 939                     generateCentersPP(data, centers, K, rng, SPP_TRIALS);
 940                 else
 941                 {
 942                     for( k = 0; k < K; k++ )
 943                         generateRandomCenter(_box, centers.ptr<float>(k), rng);
 944                 }
 945             }
 946             else
 947             {
 948                 if( iter == 0 && a == 0 && (flags & KMEANS_USE_INITIAL_LABELS) )
 949                 {
 950                     for( i = 0; i < N; i++ )
 951                         CV_Assert( (unsigned)labels[i] < (unsigned)K );
 952                 }
 953
 954                 // compute centers
 955                 centers = Scalar(0);
 956                 for( k = 0; k < K; k++ )
 957                     counters[k] = 0;
 958
 959                 for( i = 0; i < N; i++ )
 960                 {
 961                     sample = data.ptr<float>(i);
 962                     k = labels[i];
 963                     float* center = centers.ptr<float>(k);
 964                     for( j = 0; j <= dims - 4; j += 4 )
 965                     {
 966                         float t0 = center[j] + sample[j];
 967                         float t1 = center[j+1] + sample[j+1];
 968
 969                         center[j] = t0;
 970                         center[j+1] = t1;
 971
 972                         t0 = center[j+2] + sample[j+2];
 973                         t1 = center[j+3] + sample[j+3];
 974
 975                         center[j+2] = t0;
 976                         center[j+3] = t1;
 977                     }
 978                     for( ; j < dims; j++ )
 979                         center[j] += sample[j];
 980                     counters[k]++;
 981                 }
 982
 983                 if( iter > 0 )
 984                     max_center_shift = 0;
 985
 986                 for( k = 0; k < K; k++ )
 987                 {
 988                     float* center = centers.ptr<float>(k);
 989                     if( counters[k] != 0 )
 990                     {
 991                         float scale = 1.f/counters[k];
 992                         for( j = 0; j < dims; j++ )
 993                             center[j] *= scale;
 994                     }
 995                     else
 996                         generateRandomCenter(_box, center, rng);
 997
 998                     if( iter > 0 )
 999                     {
1000                         double dist = 0;
1001                         const float* old_center = old_centers.ptr<float>(k);
1002                         for( j = 0; j < dims; j++ )
1003                         {
1004                             double t = center[j] - old_center[j];
1005                             dist += t*t;
1006                         }
1007                         max_center_shift = std::max(max_center_shift, dist);
1008                     }
1009                 }
1010             }
1011
1012             // assign labels
1013             compactness = 0;
1014             for( i = 0; i < N; i++ )
1015             {
1016                 sample = data.ptr<float>(i);
1017                 int k_best = 0;
1018                 double min_dist = DBL_MAX;
1019
1020                 for( k = 0; k < K; k++ )
1021                 {
1022                     const float* center = centers.ptr<float>(k);
1023                     double dist = distance(sample, center, dims);
1024
1025                     if( min_dist > dist )
1026                     {
1027                         min_dist = dist;
1028                         k_best = k;
1029                     }
1030                 }
1031
1032                 compactness += min_dist;
1033                 labels[i] = k_best;
1034             }
1035         }
1036
1037         if( compactness < best_compactness )
1038         {
1039             best_compactness = compactness;
1040             if( _centers )
1041                 centers.copyTo(*_centers);
1042             _labels.copyTo(best_labels);
1043         }
1044     }
1045
1046     return best_compactness;
1047 }
1048
1049 }
1050
1051
1052 CV_IMPL void cvSetIdentity( CvArr* arr, CvScalar value )
1053 {
1054     cv::Mat m = cv::cvarrToMat(arr);
1055     cv::setIdentity(m, value);
1056 }
1057
1058
1059 CV_IMPL CvScalar cvTrace( const CvArr* arr )
1060 {
1061     return cv::trace(cv::cvarrToMat(arr));
1062 }
1063
1064
1065 CV_IMPL void cvTranspose( const CvArr* srcarr, CvArr* dstarr )
1066 {
1067     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
1068
1069     CV_Assert( src.rows == dst.cols && src.cols == dst.rows && src.type() == dst.type() );
1070     transpose( src, dst );
1071 }
1072
1073
1074 CV_IMPL void cvCompleteSymm( CvMat* matrix, int LtoR )
1075 {
1076     cv::Mat m(matrix);
1077     cv::completeSymm( m, LtoR != 0 );
1078 }
1079
1080
1081 CV_IMPL void cvCrossProduct( const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr )
1082 {
1083     cv::Mat srcA = cv::cvarrToMat(srcAarr), dst = cv::cvarrToMat(dstarr);
1084
1085     CV_Assert( srcA.size() == dst.size() && srcA.type() == dst.type() );
1086     srcA.cross(cv::cvarrToMat(srcBarr)).copyTo(dst);
1087 }
1088
1089
1090 CV_IMPL void
1091 cvReduce( const CvArr* srcarr, CvArr* dstarr, int dim, int op )
1092 {
1093     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
1094
1095     if( dim < 0 )
1096         dim = src.rows > dst.rows ? 0 : src.cols > dst.cols ? 1 : dst.cols == 1;
1097
1098     if( dim > 1 )
1099         CV_Error( CV_StsOutOfRange, "The reduced dimensionality index is out of range" );
1100
1101     if( (dim == 0 && (dst.cols != src.cols || dst.rows != 1)) ||
1102         (dim == 1 && (dst.rows != src.rows || dst.cols != 1)) )
1103         CV_Error( CV_StsBadSize, "The output array size is incorrect" );
1104
1105     if( src.channels() != dst.channels() )
1106         CV_Error( CV_StsUnmatchedFormats, "Input and output arrays must have the same number of channels" );
1107
1108     cv::reduce(src, dst, dim, op, dst.type());
1109 }
1110
1111
1112 CV_IMPL CvArr*
1113 cvRange( CvArr* arr, double start, double end )
1114 {
1115     int ok = 0;
1116
1117     CvMat stub, *mat = (CvMat*)arr;
1118     double delta;
1119     int type, step;
1120     double val = start;
1121     int i, j;
1122     int rows, cols;
1123
1124     if( !CV_IS_MAT(mat) )
1125         mat = cvGetMat( mat, &stub);
1126
1127     rows = mat->rows;
1128     cols = mat->cols;
1129     type = CV_MAT_TYPE(mat->type);
1130     delta = (end-start)/(rows*cols);
1131
1132     if( CV_IS_MAT_CONT(mat->type) )
1133     {
1134         cols *= rows;
1135         rows = 1;
1136         step = 1;
1137     }
1138     else
1139         step = mat->step / CV_ELEM_SIZE(type);
1140
1141     if( type == CV_32SC1 )
1142     {
1143         int* idata = mat->data.i;
1144         int ival = cvRound(val), idelta = cvRound(delta);
1145
1146         if( fabs(val - ival) < DBL_EPSILON &&
1147             fabs(delta - idelta) < DBL_EPSILON )
1148         {
1149             for( i = 0; i < rows; i++, idata += step )
1150                 for( j = 0; j < cols; j++, ival += idelta )
1151                     idata[j] = ival;
1152         }
1153         else
1154         {
1155             for( i = 0; i < rows; i++, idata += step )
1156                 for( j = 0; j < cols; j++, val += delta )
1157                     idata[j] = cvRound(val);
1158         }
1159     }
1160     else if( type == CV_32FC1 )
1161     {
1162         float* fdata = mat->data.fl;
1163         for( i = 0; i < rows; i++, fdata += step )
1164             for( j = 0; j < cols; j++, val += delta )
1165                 fdata[j] = (float)val;
1166     }
1167     else
1168         CV_Error( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" );
1169
1170     ok = 1;
1171     return ok ? arr : 0;
1172 }
1173
1174
1175 CV_IMPL void
1176 cvSort( const CvArr* _src, CvArr* _dst, CvArr* _idx, int flags )
1177 {
1178     cv::Mat src = cv::cvarrToMat(_src), dst, idx;
1179
1180     if( _idx )
1181     {
1182         cv::Mat idx0 = cv::cvarrToMat(_idx), idx = idx0;
1183         CV_Assert( src.size() == idx.size() && idx.type() == CV_32S && src.data != idx.data );
1184         cv::sortIdx( src, idx, flags );
1185         CV_Assert( idx0.data == idx.data );
1186     }
1187
1188     if( _dst )
1189     {
1190         cv::Mat dst0 = cv::cvarrToMat(_dst), dst = dst0;
1191         CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
1192         cv::sort( src, dst, flags );
1193         CV_Assert( dst0.data == dst.data );
1194     }
1195 }
1196
1197
1198 CV_IMPL int
1199 cvKMeans2( const CvArr* _samples, int cluster_count, CvArr* _labels,
1200            CvTermCriteria termcrit, int attempts, CvRNG*,
1201            int flags, CvArr* _centers, double* _compactness )
1202 {
1203     cv::Mat data = cv::cvarrToMat(_samples), labels = cv::cvarrToMat(_labels), centers;
1204     if( _centers )
1205         centers = cv::cvarrToMat(_centers);
1206     CV_Assert( labels.isContinuous() && labels.type() == CV_32S &&
1207         (labels.cols == 1 || labels.rows == 1) &&
1208         labels.cols + labels.rows - 1 == data.rows );
1209     double compactness = cv::kmeans(data, cluster_count, labels, termcrit, attempts,
1210                                     flags, _centers ? &centers : 0 );
1211     if( _compactness )
1212         *_compactness = compactness;
1213     return 1;
1214 }
1215
1216 ///////////////////////////// n-dimensional matrices ////////////////////////////
1217
1218 namespace cv
1219 {
1220
1221 //////////////////////////////// MatND ///////////////////////////////////
1222
1223 MatND::MatND(const MatND& m, const Range* ranges)
1224  : flags(MAGIC_VAL), dims(0), refcount(0), data(0), datastart(0), dataend(0)
1225 {
1226     int i, j, d = m.dims;
1227
1228     CV_Assert(ranges);
1229     for( i = 0; i < d; i++ )
1230     {
1231         Range r = ranges[i];
1232         CV_Assert( r == Range::all() ||
1233             (0 <= r.start && r.start < r.end && r.end <= m.size[i]) );
1234     }
1235     *this = m;
1236     for( i = 0; i < d; i++ )
1237     {
1238         Range r = ranges[i];
1239         if( r != Range::all() )
1240         {
1241             size[i] = r.end - r.start;
1242             data += r.start*step[i];
1243         }
1244     }
1245
1246     for( i = 0; i < d; i++ )
1247     {
1248         if( size[i] != 1 )
1249             break;
1250     }
1251
1252     CV_Assert( step[d-1] == elemSize() );
1253     for( j = d-1; j > i; j-- )
1254     {
1255         if( step[j]*size[j] < step[j-1] )
1256             break;
1257     }
1258     flags = (flags & ~CONTINUOUS_FLAG) | (j <= i ? CONTINUOUS_FLAG : 0);
1259 }
1260
1261 void MatND::create(int d, const int* _sizes, int _type)
1262 {
1263     CV_Assert(d > 0 && _sizes);
1264     int i;
1265     _type = CV_MAT_TYPE(_type);
1266     if( data && d == dims && _type == type() )
1267     {
1268         for( i = 0; i < d; i++ )
1269             if( size[i] != _sizes[i] )
1270                 break;
1271         if( i == d )
1272             return;
1273     }
1274
1275     release();
1276
1277     flags = (_type & CV_MAT_TYPE_MASK) | MAGIC_VAL | CONTINUOUS_FLAG;
1278     size_t total = elemSize();
1279     int64 total1;
1280
1281     for( i = d-1; i >= 0; i-- )
1282     {
1283         int sz = _sizes[i];
1284         size[i] = sz;
1285         step[i] = total;
1286         total1 = (int64)total*sz;
1287         CV_Assert( sz > 0 );
1288         if( (uint64)total1 != (size_t)total1 )
1289             CV_Error( CV_StsOutOfRange, "The total matrix size does not fit to \"size_t\" type" );
1290         total = (size_t)total1;
1291     }
1292     total = alignSize(total, (int)sizeof(*refcount));
1293     data = datastart = (uchar*)fastMalloc(total + (int)sizeof(*refcount));
1294     dataend = datastart + step[0]*size[0];
1295     refcount = (int*)(data + total);
1296     *refcount = 1;
1297     dims = d;
1298 }
1299
1300 void MatND::copyTo( MatND& m ) const
1301 {
1302     m.create( m.dims, m.size, type() );
1303     NAryMatNDIterator it(*this, m);
1304
1305     for( int i = 0; i < it.nplanes; i++, ++it )
1306         it.planes[0].copyTo(it.planes[1]);
1307 }
1308
1309 void MatND::copyTo( MatND& m, const MatND& mask ) const
1310 {
1311     m.create( dims, size, type() );
1312     NAryMatNDIterator it(*this, m, mask);
1313
1314     for( int i = 0; i < it.nplanes; i++, ++it )
1315         it.planes[0].copyTo(it.planes[1], it.planes[2]);
1316 }
1317
1318 void MatND::convertTo( MatND& m, int rtype, double alpha, double beta ) const
1319 {
1320     rtype = rtype < 0 ? type() : CV_MAKETYPE(CV_MAT_DEPTH(rtype), channels());
1321     m.create( dims, size, rtype );
1322     NAryMatNDIterator it(*this, m);
1323
1324     for( int i = 0; i < it.nplanes; i++, ++it )
1325         it.planes[0].convertTo(it.planes[1], rtype, alpha, beta);
1326 }
1327
1328 MatND& MatND::operator = (const Scalar& s)
1329 {
1330     NAryMatNDIterator it(*this);
1331     for( int i = 0; i < it.nplanes; i++, ++it )
1332         it.planes[0] = s;
1333
1334     return *this;
1335 }
1336
1337 MatND& MatND::setTo(const Scalar& s, const MatND& mask)
1338 {
1339     NAryMatNDIterator it(*this, mask);
1340     for( int i = 0; i < it.nplanes; i++, ++it )
1341         it.planes[0].setTo(s, it.planes[1]);
1342
1343     return *this;
1344 }
1345
1346 MatND MatND::reshape(int, int, const int*) const
1347 {
1348     CV_Error(CV_StsNotImplemented, "");
1349     // TBD
1350     return MatND();
1351 }
1352
1353 MatND::operator Mat() const
1354 {
1355     int i, d = dims, d1, rows, cols;
1356     size_t _step = Mat::AUTO_STEP;
1357
1358     if( d <= 2 )
1359     {
1360         rows = size[0];
1361         cols = d == 2 ? size[1] : 1;
1362         if( d == 2 )
1363             _step = step[0];
1364     }
1365     else
1366     {
1367         rows = 1;
1368         cols = size[d-1];
1369
1370         for( d1 = 0; d1 < d; d1++ )
1371             if( size[d1] > 1 )
1372                 break;
1373
1374         for( i = d-1; i > d1; i-- )
1375         {
1376             int64 cols1 = (int64)cols*size[i-1];
1377             if( cols1 != (int)cols1 || size[i]*step[i] != step[i-1] )
1378                 break;
1379             cols = (int)cols1;
1380         }
1381
1382         if( i > d1 )
1383         {
1384             --i;
1385             _step = step[i];
1386             rows = size[i];
1387             for( ; i > d1; i-- )
1388             {
1389                 int64 rows1 = (int64)rows*size[i-1];
1390                 if( rows1 != (int)rows1 || size[i]*step[i] != step[i-1] )
1391                     break;
1392                 rows = (int)rows1;
1393             }
1394
1395             if( i > d1 )
1396                 CV_Error( CV_StsBadArg,
1397                 "The nD matrix can not be represented as 2D matrix due "
1398                 "to its layout in memory; you may use (Mat)the_matnd.clone() instead" );
1399         }
1400     }
1401
1402     Mat m(rows, cols, type(), data, _step);
1403     m.datastart = datastart;
1404     m.dataend = dataend;
1405     m.refcount = refcount;
1406     m.addref();
1407     return m;
1408 }
1409
1410 MatND::operator CvMatND() const
1411 {
1412     CvMatND mat;
1413     cvInitMatNDHeader( &mat, dims, size, type(), data );
1414     int i, d = dims;
1415     for( i = 0; i < d; i++ )
1416         mat.dim[i].step = (int)step[i];
1417     mat.type |= flags & CONTINUOUS_FLAG;
1418     return mat;
1419 }
1420
1421 NAryMatNDIterator::NAryMatNDIterator(const MatND** _arrays, size_t count)
1422 {
1423     init(_arrays, count);
1424 }
1425
1426 NAryMatNDIterator::NAryMatNDIterator(const MatND* _arrays, size_t count)
1427 {
1428     AutoBuffer<const MatND*, 32> buf(count);
1429     for( size_t i = 0; i < count; i++ )
1430         buf[i] = _arrays + i;
1431     init(buf, count);
1432 }
1433
1434
1435 NAryMatNDIterator::NAryMatNDIterator(const MatND& m1)
1436 {
1437     const MatND* mm[] = {&m1};
1438     init(mm, 1);
1439 }
1440
1441 NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2)
1442 {
1443     const MatND* mm[] = {&m1, &m2};
1444     init(mm, 2);
1445 }
1446
1447 NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2, const MatND& m3)
1448 {
1449     const MatND* mm[] = {&m1, &m2, &m3};
1450     init(mm, 3);
1451 }
1452
1453 NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2,
1454                                      const MatND& m3, const MatND& m4)
1455 {
1456     const MatND* mm[] = {&m1, &m2, &m3, &m4};
1457     init(mm, 4);
1458 }
1459
1460 NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2,
1461                                      const MatND& m3, const MatND& m4,
1462                                      const MatND& m5)
1463 {
1464     const MatND* mm[] = {&m1, &m2, &m3, &m4, &m5};
1465     init(mm, 5);
1466 }
1467
1468 NAryMatNDIterator::NAryMatNDIterator(const MatND& m1, const MatND& m2,
1469                                      const MatND& m3, const MatND& m4,
1470                                      const MatND& m5, const MatND& m6)
1471 {
1472     const MatND* mm[] = {&m1, &m2, &m3, &m4, &m5, &m6};
1473     init(mm, 6);
1474 }
1475
1476 void NAryMatNDIterator::init(const MatND** _arrays, size_t count)
1477 {
1478     CV_Assert( _arrays && count > 0 );
1479     arrays.resize(count);
1480     int i, j, d1=0, i0 = -1, d = -1, n = (int)count;
1481     size_t esz = 0;
1482
1483     iterdepth = 0;
1484
1485     for( i = 0; i < n; i++ )
1486     {
1487         if( !_arrays[i] || !_arrays[i]->data )
1488         {
1489             arrays[i] = MatND();
1490             continue;
1491         }
1492         const MatND& A = arrays[i] = *_arrays[i];
1493
1494         if( i0 < 0 )
1495         {
1496             i0 = i;
1497             d = A.dims;
1498             esz = A.elemSize();
1499
1500             // find the first dimensionality which is different from 1;
1501             // in any of the arrays the first "d1" steps do not affect the continuity
1502             for( d1 = 0; d1 < d; d1++ )
1503                 if( A.size[d1] > 1 )
1504                     break;
1505         }
1506         else
1507         {
1508             CV_Assert( A.dims == d );
1509             for( j = 0; j < d; j++ )
1510                 CV_Assert( A.size[j] == arrays[i0].size[j] );
1511         }
1512
1513         if( !A.isContinuous() )
1514         {
1515             CV_Assert( A.step[d-1] == esz );
1516             for( j = d-1; j > d1; j-- )
1517                 if( A.step[j]*A.size[j] < A.step[j-1] )
1518                     break;
1519             iterdepth = std::max(iterdepth, j);
1520         }
1521     }
1522
1523     if( i0 < 0 )
1524         CV_Error( CV_StsBadArg, "All the input arrays are empty" );
1525
1526     int total = arrays[i0].size[d-1];
1527     for( j = d-1; j > iterdepth; j-- )
1528     {
1529         int64 total1 = (int64)total*arrays[i0].size[j-1];
1530         if( total1 != (int)total1 )
1531             break;
1532         total = (int)total1;
1533     }
1534
1535     iterdepth = j;
1536     if( iterdepth == d1 )
1537         iterdepth = 0;
1538
1539     planes.resize(n);
1540     for( i = 0; i < n; i++ )
1541     {
1542         if( !arrays[i].data )
1543         {
1544             planes[i] = Mat();
1545             continue;
1546         }
1547         planes[i] = Mat( 1, total, arrays[i].type(), arrays[i].data );
1548         planes[i].datastart = arrays[i].datastart;
1549         planes[i].dataend = arrays[i].dataend;
1550         planes[i].refcount = arrays[i].refcount;
1551         planes[i].addref();
1552     }
1553
1554     idx = 0;
1555     nplanes = 1;
1556     for( j = iterdepth-1; j >= 0; j-- )
1557         nplanes *= arrays[i0].size[j];
1558 }
1559
1560
1561 NAryMatNDIterator& NAryMatNDIterator::operator ++()
1562 {
1563     if( idx >= nplanes-1 )
1564         return *this;
1565     ++idx;
1566
1567     for( size_t i = 0; i < arrays.size(); i++ )
1568     {
1569         const MatND& A = arrays[i];
1570         Mat& M = planes[i];
1571         if( !A.data )
1572             continue;
1573         int _idx = idx;
1574         uchar* data = A.data;
1575         for( int j = iterdepth-1; j >= 0 && _idx > 0; j-- )
1576         {
1577             int szi = A.size[j], t = _idx/szi;
1578             data += (_idx - t * szi)*A.step[j];
1579             _idx = t;
1580         }
1581         M.data = data;
1582     }
1583
1584     return *this;
1585 }
1586
1587 NAryMatNDIterator NAryMatNDIterator::operator ++(int)
1588 {
1589     NAryMatNDIterator it = *this;
1590     ++*this;
1591     return it;
1592 }
1593
1594 void add(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
1595 {
1596     c.create(a.dims, a.size, a.type());
1597     NAryMatNDIterator it(a, b, c, mask);
1598
1599     for( int i = 0; i < it.nplanes; i++, ++it )
1600         add( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
1601 }
1602
1603 void subtract(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
1604 {
1605     c.create(a.dims, a.size, a.type());
1606     NAryMatNDIterator it(a, b, c, mask);
1607
1608     for( int i = 0; i < it.nplanes; i++, ++it )
1609         subtract( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
1610 }
1611
1612 void add(const MatND& a, const MatND& b, MatND& c)
1613 {
1614     c.create(a.dims, a.size, a.type());
1615     NAryMatNDIterator it(a, b, c);
1616
1617     for( int i = 0; i < it.nplanes; i++, ++it )
1618         add( it.planes[0], it.planes[1], it.planes[2] );
1619 }
1620
1621
1622 void subtract(const MatND& a, const MatND& b, MatND& c)
1623 {
1624     c.create(a.dims, a.size, a.type());
1625     NAryMatNDIterator it(a, b, c);
1626
1627     for( int i = 0; i < it.nplanes; i++, ++it )
1628         subtract( it.planes[0], it.planes[1], it.planes[2] );
1629 }
1630
1631 void add(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
1632 {
1633     c.create(a.dims, a.size, a.type());
1634     NAryMatNDIterator it(a, c, mask);
1635
1636     for( int i = 0; i < it.nplanes; i++, ++it )
1637         add( it.planes[0], s, it.planes[1], it.planes[2] );
1638 }
1639
1640 void subtract(const Scalar& s, const MatND& a, MatND& c, const MatND& mask)
1641 {
1642     c.create(a.dims, a.size, a.type());
1643     NAryMatNDIterator it(a, c, mask);
1644
1645     for( int i = 0; i < it.nplanes; i++, ++it )
1646         subtract( s, it.planes[0], it.planes[1], it.planes[2] );
1647 }
1648
1649 void multiply(const MatND& a, const MatND& b, MatND& c, double scale)
1650 {
1651     c.create(a.dims, a.size, a.type());
1652     NAryMatNDIterator it(a, b, c);
1653
1654     for( int i = 0; i < it.nplanes; i++, ++it )
1655         multiply( it.planes[0], it.planes[1], it.planes[2], scale );
1656 }
1657
1658 void divide(const MatND& a, const MatND& b, MatND& c, double scale)
1659 {
1660     c.create(a.dims, a.size, a.type());
1661     NAryMatNDIterator it(a, b, c);
1662
1663     for( int i = 0; i < it.nplanes; i++, ++it )
1664         divide( it.planes[0], it.planes[1], it.planes[2], scale );
1665 }
1666
1667 void divide(double scale, const MatND& b, MatND& c)
1668 {
1669     c.create(b.dims, b.size, b.type());
1670     NAryMatNDIterator it(b, c);
1671
1672     for( int i = 0; i < it.nplanes; i++, ++it )
1673         divide( scale, it.planes[0], it.planes[1] );
1674 }
1675
1676 void scaleAdd(const MatND& a, double alpha, const MatND& b, MatND& c)
1677 {
1678     c.create(a.dims, a.size, a.type());
1679     NAryMatNDIterator it(a, b, c);
1680
1681     for( int i = 0; i < it.nplanes; i++, ++it )
1682         scaleAdd( it.planes[0], alpha, it.planes[1], it.planes[2] );
1683 }
1684
1685 void addWeighted(const MatND& a, double alpha, const MatND& b,
1686                  double beta, double gamma, MatND& c)
1687 {
1688     c.create(a.dims, a.size, a.type());
1689     NAryMatNDIterator it(a, b, c);
1690
1691     for( int i = 0; i < it.nplanes; i++, ++it )
1692         addWeighted( it.planes[0], alpha, it.planes[1], beta, gamma, it.planes[2] );
1693 }
1694
1695 Scalar sum(const MatND& m)
1696 {
1697     NAryMatNDIterator it(m);
1698     Scalar s;
1699
1700     for( int i = 0; i < it.nplanes; i++, ++it )
1701         s += sum(it.planes[0]);
1702     return s;
1703 }
1704
1705 int countNonZero( const MatND& m )
1706 {
1707     NAryMatNDIterator it(m);
1708     int nz = 0;
1709
1710     for( int i = 0; i < it.nplanes; i++, ++it )
1711         nz += countNonZero(it.planes[0]);
1712     return nz;
1713 }
1714
1715 Scalar mean(const MatND& m)
1716 {
1717     NAryMatNDIterator it(m);
1718     double total = 1;
1719     for( int i = 0; i < m.dims; i++ )
1720         total *= m.size[i];
1721     return sum(m)*(1./total);
1722 }
1723
1724 Scalar mean(const MatND& m, const MatND& mask)
1725 {
1726     if( !mask.data )
1727         return mean(m);
1728     NAryMatNDIterator it(m, mask);
1729     double total = 0;
1730     Scalar s;
1731     for( int i = 0; i < it.nplanes; i++, ++it )
1732     {
1733         s += sum(it.planes[0]);
1734         total += countNonZero(it.planes[1]);
1735     }
1736     return s *= std::max(total, 1.);
1737 }
1738
1739 void meanStdDev(const MatND& m, Scalar& mean, Scalar& stddev, const MatND& mask)
1740 {
1741     NAryMatNDIterator it(m, mask);
1742     double total = 0;
1743     Scalar s, sq;
1744     int k, cn = m.channels();
1745
1746     for( int i = 0; i < it.nplanes; i++, ++it )
1747     {
1748         Scalar _mean, _stddev;
1749         meanStdDev(it.planes[0], _mean, _stddev, it.planes[1]);
1750         double nz = mask.data ? countNonZero(it.planes[1]) :
1751             (double)it.planes[0].rows*it.planes[0].cols;
1752         for( k = 0; k < cn; k++ )
1753         {
1754             s[k] += _mean[k]*nz;
1755             sq[k] += (_stddev[k]*_stddev[k] + _mean[k]*_mean[k])*nz;
1756         }
1757         total += nz;
1758     }
1759
1760     mean = stddev = Scalar();
1761     total = 1./std::max(total, 1.);
1762     for( k = 0; k < cn; k++ )
1763     {
1764         mean[k] = s[k]*total;
1765         stddev[k] = std::sqrt(std::max(sq[k]*total - mean[k]*mean[k], 0.));
1766     }
1767 }
1768
1769 double norm(const MatND& a, int normType, const MatND& mask)
1770 {
1771     NAryMatNDIterator it(a, mask);
1772     double total = 0;
1773
1774     for( int i = 0; i < it.nplanes; i++, ++it )
1775     {
1776         double n = norm(it.planes[0], normType, it.planes[1]);
1777         if( normType == NORM_INF )
1778             total = std::max(total, n);
1779         else if( normType == NORM_L1 )
1780             total += n;
1781         else
1782             total += n*n;
1783     }
1784
1785     return normType != NORM_L2 ? total : std::sqrt(total);
1786 }
1787
1788 double norm(const MatND& a, const MatND& b,
1789             int normType, const MatND& mask)
1790 {
1791     bool isRelative = (normType & NORM_RELATIVE) != 0;
1792     normType &= 7;
1793
1794     NAryMatNDIterator it(a, b, mask);
1795     double num = 0, denom = 0;
1796
1797     for( int i = 0; i < it.nplanes; i++, ++it )
1798     {
1799         double n = norm(it.planes[0], it.planes[1], normType, it.planes[2]);
1800         double d = !isRelative ? 0 : norm(it.planes[1], normType, it.planes[2]);
1801         if( normType == NORM_INF )
1802         {
1803             num = std::max(num, n);
1804             denom = std::max(denom, d);
1805         }
1806         else if( normType == NORM_L1 )
1807         {
1808             num += n;
1809             denom += d;
1810         }
1811         else
1812         {
1813             num += n*n;
1814             denom += d*d;
1815         }
1816     }
1817
1818     if( normType == NORM_L2 )
1819     {
1820         num = std::sqrt(num);
1821         denom = std::sqrt(denom);
1822     }
1823
1824     return !isRelative ? num : num/std::max(denom,DBL_EPSILON);
1825 }
1826
1827 void normalize( const MatND& src, MatND& dst, double a, double b,
1828                 int norm_type, int rtype, const MatND& mask )
1829 {
1830     double scale = 1, shift = 0;
1831     if( norm_type == CV_MINMAX )
1832     {
1833         double smin = 0, smax = 0;
1834         double dmin = std::min( a, b ), dmax = std::max( a, b );
1835         minMaxLoc( src, &smin, &smax, 0, 0, mask );
1836         scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0);
1837         shift = dmin - smin*scale;
1838     }
1839     else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
1840     {
1841         scale = norm( src, norm_type, mask );
1842         scale = scale > DBL_EPSILON ? a/scale : 0.;
1843         shift = 0;
1844     }
1845     else
1846         CV_Error( CV_StsBadArg, "Unknown/unsupported norm type" );
1847
1848     if( !mask.data )
1849         src.convertTo( dst, rtype, scale, shift );
1850     else
1851     {
1852         MatND temp;
1853         src.convertTo( temp, rtype, scale, shift );
1854         temp.copyTo( dst, mask );
1855     }
1856 }
1857
1858 static void ofs2idx(const MatND& a, size_t ofs, int* idx)
1859 {
1860     int i, d = a.dims;
1861     for( i = 0; i < d; i++ )
1862     {
1863         idx[i] = (int)(ofs / a.step[i]);
1864         ofs %= a.step[i];
1865     }
1866 }
1867
1868
1869 void minMaxLoc(const MatND& a, double* minVal,
1870                double* maxVal, int* minLoc, int* maxLoc,
1871                const MatND& mask)
1872 {
1873     NAryMatNDIterator it(a, mask);
1874     double minval = DBL_MAX, maxval = -DBL_MAX;
1875     size_t minofs = 0, maxofs = 0, esz = a.elemSize();
1876
1877     for( int i = 0; i < it.nplanes; i++, ++it )
1878     {
1879         double val0 = 0, val1 = 0;
1880         Point pt0, pt1;
1881         minMaxLoc( it.planes[0], &val0, &val1, &pt0, &pt1, it.planes[1] );
1882         if( val0 < minval )
1883         {
1884             minval = val0;
1885             minofs = (it.planes[0].data - a.data) + pt0.x*esz;
1886         }
1887         if( val1 > minval )
1888         {
1889             maxval = val1;
1890             maxofs = (it.planes[0].data - a.data) + pt1.x*esz;
1891         }
1892     }
1893
1894     if( minVal )
1895         *minVal = minval;
1896     if( maxVal )
1897         *maxVal = maxval;
1898     if( minLoc )
1899         ofs2idx(a, minofs, minLoc);
1900     if( maxLoc )
1901         ofs2idx(a, maxofs, maxLoc);
1902 }
1903
1904 void merge(const MatND* mv, size_t n, MatND& dst)
1905 {
1906     size_t k;
1907     CV_Assert( n > 0 );
1908     vector<MatND> v(n + 1);
1909     int total_cn = 0;
1910     for( k = 0; k < n; k++ )
1911     {
1912         total_cn += mv[k].channels();
1913         v[k] = mv[k];
1914     }
1915     dst.create( mv[0].dims, mv[0].size, CV_MAKETYPE(mv[0].depth(), total_cn) );
1916     v[n] = dst;
1917     NAryMatNDIterator it(&v[0], v.size());
1918
1919     for( int i = 0; i < it.nplanes; i++, ++it )
1920         merge( &it.planes[0], n, it.planes[n] );
1921 }
1922
1923 void split(const MatND& m, MatND* mv)
1924 {
1925     size_t k, n = m.channels();
1926     CV_Assert( n > 0 );
1927     vector<MatND> v(n + 1);
1928     for( k = 0; k < n; k++ )
1929     {
1930         mv[k].create( m.dims, m.size, CV_MAKETYPE(m.depth(), 1) );
1931         v[k] = mv[k];
1932     }
1933     v[n] = m;
1934     NAryMatNDIterator it(&v[0], v.size());
1935
1936     for( int i = 0; i < it.nplanes; i++, ++it )
1937         split( it.planes[n], &it.planes[0] );
1938 }
1939
1940 void mixChannels(const MatND* src, int nsrcs, MatND* dst, int ndsts,
1941                  const int* fromTo, size_t npairs)
1942 {
1943     size_t k, m = nsrcs, n = ndsts;
1944     CV_Assert( n > 0 && m > 0 );
1945     vector<MatND> v(m + n);
1946     for( k = 0; k < m; k++ )
1947         v[k] = src[k];
1948     for( k = 0; k < n; k++ )
1949         v[m + k] = dst[k];
1950     NAryMatNDIterator it(&v[0], v.size());
1951
1952     for( int i = 0; i < it.nplanes; i++, ++it )
1953     {
1954         Mat* pptr = &it.planes[0];
1955         mixChannels( pptr, m, pptr + m, n, fromTo, npairs );
1956     }
1957 }
1958
1959 void bitwise_and(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
1960 {
1961     c.create(a.dims, a.size, a.type());
1962     NAryMatNDIterator it(a, b, c, mask);
1963
1964     for( int i = 0; i < it.nplanes; i++, ++it )
1965         bitwise_and( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
1966 }
1967
1968 void bitwise_or(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
1969 {
1970     c.create(a.dims, a.size, a.type());
1971     NAryMatNDIterator it(a, b, c, mask);
1972
1973     for( int i = 0; i < it.nplanes; i++, ++it )
1974         bitwise_or( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
1975 }
1976
1977 void bitwise_xor(const MatND& a, const MatND& b, MatND& c, const MatND& mask)
1978 {
1979     c.create(a.dims, a.size, a.type());
1980     NAryMatNDIterator it(a, b, c, mask);
1981
1982     for( int i = 0; i < it.nplanes; i++, ++it )
1983         bitwise_xor( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
1984 }
1985
1986 void bitwise_and(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
1987 {
1988     c.create(a.dims, a.size, a.type());
1989     NAryMatNDIterator it(a, c, mask);
1990
1991     for( int i = 0; i < it.nplanes; i++, ++it )
1992         bitwise_and( it.planes[0], s, it.planes[1], it.planes[2] );
1993 }
1994
1995 void bitwise_or(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
1996 {
1997     c.create(a.dims, a.size, a.type());
1998     NAryMatNDIterator it(a, c, mask);
1999
2000     for( int i = 0; i < it.nplanes; i++, ++it )
2001         bitwise_or( it.planes[0], s, it.planes[1], it.planes[2] );
2002 }
2003
2004 void bitwise_xor(const MatND& a, const Scalar& s, MatND& c, const MatND& mask)
2005 {
2006     c.create(a.dims, a.size, a.type());
2007     NAryMatNDIterator it(a, c, mask);
2008
2009     for( int i = 0; i < it.nplanes; i++, ++it )
2010         bitwise_xor( it.planes[0], s, it.planes[1], it.planes[2] );
2011 }
2012
2013 void bitwise_not(const MatND& a, MatND& c)
2014 {
2015     c.create(a.dims, a.size, a.type());
2016     NAryMatNDIterator it(a, c);
2017
2018     for( int i = 0; i < it.nplanes; i++, ++it )
2019         bitwise_not( it.planes[0], it.planes[1] );
2020 }
2021
2022 void absdiff(const MatND& a, const MatND& b, MatND& c)
2023 {
2024     c.create(a.dims, a.size, a.type());
2025     NAryMatNDIterator it(a, b, c);
2026
2027     for( int i = 0; i < it.nplanes; i++, ++it )
2028         absdiff( it.planes[0], it.planes[1], it.planes[2] );
2029 }
2030
2031 void absdiff(const MatND& a, const Scalar& s, MatND& c)
2032 {
2033     c.create(a.dims, a.size, a.type());
2034     NAryMatNDIterator it(a, c);
2035
2036     for( int i = 0; i < it.nplanes; i++, ++it )
2037         absdiff( it.planes[0], s, it.planes[1] );
2038 }
2039
2040 void inRange(const MatND& src, const MatND& lowerb,
2041              const MatND& upperb, MatND& dst)
2042 {
2043     dst.create(src.dims, src.size, CV_8UC1);
2044     NAryMatNDIterator it(src, lowerb, upperb, dst);
2045
2046     for( int i = 0; i < it.nplanes; i++, ++it )
2047         inRange( it.planes[0], it.planes[1], it.planes[2], it.planes[3] );
2048 }
2049
2050 void inRange(const MatND& src, const Scalar& lowerb,
2051              const Scalar& upperb, MatND& dst)
2052 {
2053     dst.create(src.dims, src.size, CV_8UC1);
2054     NAryMatNDIterator it(src, dst);
2055
2056     for( int i = 0; i < it.nplanes; i++, ++it )
2057         inRange( it.planes[0], lowerb, upperb, it.planes[1] );
2058 }
2059
2060 void compare(const MatND& a, const MatND& b, MatND& c, int cmpop)
2061 {
2062     c.create(a.dims, a.size, CV_8UC1);
2063     NAryMatNDIterator it(a, b, c);
2064
2065     for( int i = 0; i < it.nplanes; i++, ++it )
2066         compare( it.planes[0], it.planes[1], it.planes[2], cmpop );
2067 }
2068
2069 void compare(const MatND& a, double s, MatND& c, int cmpop)
2070 {
2071     c.create(a.dims, a.size, CV_8UC1);
2072     NAryMatNDIterator it(a, c);
2073
2074     for( int i = 0; i < it.nplanes; i++, ++it )
2075         compare( it.planes[0], s, it.planes[1], cmpop );
2076 }
2077
2078 void min(const MatND& a, const MatND& b, MatND& c)
2079 {
2080     c.create(a.dims, a.size, a.type());
2081     NAryMatNDIterator it(a, b, c);
2082
2083     for( int i = 0; i < it.nplanes; i++, ++it )
2084         min( it.planes[0], it.planes[1], it.planes[2] );
2085 }
2086
2087 void min(const MatND& a, double alpha, MatND& c)
2088 {
2089     c.create(a.dims, a.size, a.type());
2090     NAryMatNDIterator it(a, c);
2091
2092     for( int i = 0; i < it.nplanes; i++, ++it )
2093         min( it.planes[0], alpha, it.planes[1] );
2094 }
2095
2096 void max(const MatND& a, const MatND& b, MatND& c)
2097 {
2098     c.create(a.dims, a.size, a.type());
2099     NAryMatNDIterator it(a, b, c);
2100
2101     for( int i = 0; i < it.nplanes; i++, ++it )
2102         max( it.planes[0], it.planes[1], it.planes[2] );
2103 }
2104
2105 void max(const MatND& a, double alpha, MatND& c)
2106 {
2107     c.create(a.dims, a.size, a.type());
2108     NAryMatNDIterator it(a, c);
2109
2110     for( int i = 0; i < it.nplanes; i++, ++it )
2111         max( it.planes[0], alpha, it.planes[1] );
2112 }
2113
2114 void sqrt(const MatND& a, MatND& b)
2115 {
2116     b.create(a.dims, a.size, a.type());
2117     NAryMatNDIterator it(a, b);
2118
2119     for( int i = 0; i < it.nplanes; i++, ++it )
2120         sqrt( it.planes[0], it.planes[1] );
2121 }
2122
2123 void pow(const MatND& a, double power, MatND& b)
2124 {
2125     b.create(a.dims, a.size, a.type());
2126     NAryMatNDIterator it(a, b);
2127
2128     for( int i = 0; i < it.nplanes; i++, ++it )
2129         pow( it.planes[0], power, it.planes[1] );
2130 }
2131
2132 void exp(const MatND& a, MatND& b)
2133 {
2134     b.create(a.dims, a.size, a.type());
2135     NAryMatNDIterator it(a, b);
2136
2137     for( int i = 0; i < it.nplanes; i++, ++it )
2138         exp( it.planes[0], it.planes[1] );
2139 }
2140
2141 void log(const MatND& a, MatND& b)
2142 {
2143     b.create(a.dims, a.size, a.type());
2144     NAryMatNDIterator it(a, b);
2145
2146     for( int i = 0; i < it.nplanes; i++, ++it )
2147         log( it.planes[0], it.planes[1] );
2148 }
2149
2150 bool checkRange(const MatND& a, bool quiet, int*,
2151                 double minVal, double maxVal)
2152 {
2153     NAryMatNDIterator it(a);
2154
2155     for( int i = 0; i < it.nplanes; i++, ++it )
2156     {
2157         Point pt;
2158         if( !checkRange( it.planes[0], quiet, &pt, minVal, maxVal ))
2159         {
2160             // todo: set index properly
2161             return false;
2162         }
2163     }
2164     return true;
2165 }
2166
2167
2168 //////////////////////////////// SparseMat ////////////////////////////////
2169
2170 template<typename T1, typename T2> void
2171 convertData_(const void* _from, void* _to, int cn)
2172 {
2173     const T1* from = (const T1*)_from;
2174     T2* to = (T2*)_to;
2175     if( cn == 1 )
2176         *to = saturate_cast<T2>(*from);
2177     else
2178         for( int i = 0; i < cn; i++ )
2179             to[i] = saturate_cast<T2>(from[i]);
2180 }
2181
2182 template<typename T1, typename T2> void
2183 convertScaleData_(const void* _from, void* _to, int cn, double alpha, double beta)
2184 {
2185     const T1* from = (const T1*)_from;
2186     T2* to = (T2*)_to;
2187     if( cn == 1 )
2188         *to = saturate_cast<T2>(*from*alpha + beta);
2189     else
2190         for( int i = 0; i < cn; i++ )
2191             to[i] = saturate_cast<T2>(from[i]*alpha + beta);
2192 }
2193
2194 ConvertData getConvertData(int fromType, int toType)
2195 {
2196     static ConvertData tab[][8] =
2197     {{ convertData_<uchar, uchar>, convertData_<uchar, schar>,
2198       convertData_<uchar, ushort>, convertData_<uchar, short>,
2199       convertData_<uchar, int>, convertData_<uchar, float>,
2200       convertData_<uchar, double>, 0 },
2201
2202     { convertData_<schar, uchar>, convertData_<schar, schar>,
2203       convertData_<schar, ushort>, convertData_<schar, short>,
2204       convertData_<schar, int>, convertData_<schar, float>,
2205       convertData_<schar, double>, 0 },
2206
2207     { convertData_<ushort, uchar>, convertData_<ushort, schar>,
2208       convertData_<ushort, ushort>, convertData_<ushort, short>,
2209       convertData_<ushort, int>, convertData_<ushort, float>,
2210       convertData_<ushort, double>, 0 },
2211
2212     { convertData_<short, uchar>, convertData_<short, schar>,
2213       convertData_<short, ushort>, convertData_<short, short>,
2214       convertData_<short, int>, convertData_<short, float>,
2215       convertData_<short, double>, 0 },
2216
2217     { convertData_<int, uchar>, convertData_<int, schar>,
2218       convertData_<int, ushort>, convertData_<int, short>,
2219       convertData_<int, int>, convertData_<int, float>,
2220       convertData_<int, double>, 0 },
2221
2222     { convertData_<float, uchar>, convertData_<float, schar>,
2223       convertData_<float, ushort>, convertData_<float, short>,
2224       convertData_<float, int>, convertData_<float, float>,
2225       convertData_<float, double>, 0 },
2226
2227     { convertData_<double, uchar>, convertData_<double, schar>,
2228       convertData_<double, ushort>, convertData_<double, short>,
2229       convertData_<double, int>, convertData_<double, float>,
2230       convertData_<double, double>, 0 },
2231
2232     { 0, 0, 0, 0, 0, 0, 0, 0 }};
2233
2234     ConvertData func = tab[CV_MAT_DEPTH(fromType)][CV_MAT_DEPTH(toType)];
2235     CV_Assert( func != 0 );
2236     return func;
2237 }
2238
2239 ConvertScaleData getConvertScaleData(int fromType, int toType)
2240 {
2241     static ConvertScaleData tab[][8] =
2242     {{ convertScaleData_<uchar, uchar>, convertScaleData_<uchar, schar>,
2243       convertScaleData_<uchar, ushort>, convertScaleData_<uchar, short>,
2244       convertScaleData_<uchar, int>, convertScaleData_<uchar, float>,
2245       convertScaleData_<uchar, double>, 0 },
2246
2247     { convertScaleData_<schar, uchar>, convertScaleData_<schar, schar>,
2248       convertScaleData_<schar, ushort>, convertScaleData_<schar, short>,
2249       convertScaleData_<schar, int>, convertScaleData_<schar, float>,
2250       convertScaleData_<schar, double>, 0 },
2251
2252     { convertScaleData_<ushort, uchar>, convertScaleData_<ushort, schar>,
2253       convertScaleData_<ushort, ushort>, convertScaleData_<ushort, short>,
2254       convertScaleData_<ushort, int>, convertScaleData_<ushort, float>,
2255       convertScaleData_<ushort, double>, 0 },
2256
2257     { convertScaleData_<short, uchar>, convertScaleData_<short, schar>,
2258       convertScaleData_<short, ushort>, convertScaleData_<short, short>,
2259       convertScaleData_<short, int>, convertScaleData_<short, float>,
2260       convertScaleData_<short, double>, 0 },
2261
2262     { convertScaleData_<int, uchar>, convertScaleData_<int, schar>,
2263       convertScaleData_<int, ushort>, convertScaleData_<int, short>,
2264       convertScaleData_<int, int>, convertScaleData_<int, float>,
2265       convertScaleData_<int, double>, 0 },
2266
2267     { convertScaleData_<float, uchar>, convertScaleData_<float, schar>,
2268       convertScaleData_<float, ushort>, convertScaleData_<float, short>,
2269       convertScaleData_<float, int>, convertScaleData_<float, float>,
2270       convertScaleData_<float, double>, 0 },
2271
2272     { convertScaleData_<double, uchar>, convertScaleData_<double, schar>,
2273       convertScaleData_<double, ushort>, convertScaleData_<double, short>,
2274       convertScaleData_<double, int>, convertScaleData_<double, float>,
2275       convertScaleData_<double, double>, 0 },
2276
2277     { 0, 0, 0, 0, 0, 0, 0, 0 }};
2278
2279     ConvertScaleData func = tab[CV_MAT_DEPTH(fromType)][CV_MAT_DEPTH(toType)];
2280     CV_Assert( func != 0 );
2281     return func;
2282 }
2283
2284 enum { HASH_SIZE0 = 8 };
2285
2286 static inline void copyElem(const uchar* from, uchar* to, size_t elemSize)
2287 {
2288     size_t i;
2289     for( i = 0; i <= elemSize - sizeof(int); i += sizeof(int) )
2290         *(int*)(to + i) = *(const int*)(from + i);
2291     for( ; i < elemSize; i++ )
2292         to[i] = from[i];
2293 }
2294
2295 static inline bool isZeroElem(const uchar* data, size_t elemSize)
2296 {
2297     size_t i;
2298     for( i = 0; i <= elemSize - sizeof(int); i += sizeof(int) )
2299         if( *(int*)(data + i) != 0 )
2300             return false;
2301     for( ; i < elemSize; i++ )
2302         if( data[i] != 0 )
2303             return false;
2304     return true;
2305 }
2306
2307 SparseMat::Hdr::Hdr( int _dims, const int* _sizes, int _type )
2308 {
2309     refcount = 1;
2310
2311     dims = _dims;
2312     valueOffset = (int)alignSize(sizeof(SparseMat::Node) +
2313         sizeof(int)*(dims - CV_MAX_DIM), CV_ELEM_SIZE1(_type));
2314     nodeSize = alignSize(valueOffset +
2315         CV_ELEM_SIZE(_type), (int)sizeof(size_t));
2316
2317     int i;
2318     for( i = 0; i < dims; i++ )
2319         size[i] = _sizes[i];
2320     for( ; i < CV_MAX_DIM; i++ )
2321         size[i] = 0;
2322     clear();
2323 }
2324
2325 void SparseMat::Hdr::clear()
2326 {
2327     hashtab.clear();
2328     hashtab.resize(HASH_SIZE0);
2329     pool.clear();
2330     pool.resize(nodeSize);
2331     nodeCount = freeList = 0;
2332 }
2333
2334
2335 SparseMat::SparseMat(const Mat& m, bool try1d)
2336 : flags(MAGIC_VAL), hdr(0)
2337 {
2338     bool is1d = try1d && m.cols == 1;
2339
2340     if( is1d )
2341     {
2342         int i, M = m.rows;
2343         const uchar* data = m.data;
2344         size_t step =  m.step, esz = m.elemSize();
2345         create( 1, &M, m.type() );
2346         for( i = 0; i < M; i++ )
2347         {
2348             const uchar* from = data + step*i;
2349             if( isZeroElem(from, esz) )
2350                 continue;
2351             uchar* to = newNode(&i, hash(i));
2352             copyElem(from, to, esz);
2353         }
2354     }
2355     else
2356     {
2357         int i, j, size[] = {m.rows, m.cols};
2358         const uchar* data = m.data;
2359         size_t step =  m.step, esz = m.elemSize();
2360         create( 2, size, m.type() );
2361         for( i = 0; i < m.rows; i++ )
2362         {
2363             for( j = 0; j < m.cols; j++ )
2364             {
2365                 const uchar* from = data + step*i + esz*j;
2366                 if( isZeroElem(from, esz) )
2367                     continue;
2368                 int idx[] = {i, j};
2369                 uchar* to = newNode(idx, hash(i, j));
2370                 copyElem(from, to, esz);
2371             }
2372         }
2373     }
2374 }
2375
2376 SparseMat::SparseMat(const MatND& m)
2377 : flags(MAGIC_VAL), hdr(0)
2378 {
2379     create( m.dims, m.size, m.type() );
2380
2381     int i, idx[CV_MAX_DIM] = {0}, d = m.dims, lastSize = m.size[d - 1];
2382     size_t esz = m.elemSize();
2383     uchar* ptr = m.data;
2384
2385     for(;;)
2386     {
2387         for( i = 0; i < lastSize; i++, ptr += esz )
2388         {
2389             if( isZeroElem(ptr, esz) )
2390                 continue;
2391             idx[d-1] = i;
2392             uchar* to = newNode(idx, hash(idx));
2393             copyElem( ptr, to, esz );
2394         }
2395
2396         for( i = d - 2; i >= 0; i-- )
2397         {
2398             ptr += m.step[i] - m.size[i+1]*m.step[i+1];
2399             if( ++idx[i] < m.size[i] )
2400                 break;
2401             idx[i] = 0;
2402         }
2403         if( i < 0 )
2404             break;
2405     }
2406 }
2407
2408 SparseMat::SparseMat(const CvSparseMat* m)
2409 : flags(MAGIC_VAL), hdr(0)
2410 {
2411     CV_Assert(m);
2412     create( m->dims, &m->size[0], m->type );
2413
2414     CvSparseMatIterator it;
2415     CvSparseNode* n = cvInitSparseMatIterator(m, &it);
2416     size_t esz = elemSize();
2417
2418     for( ; n != 0; n = cvGetNextSparseNode(&it) )
2419     {
2420         const int* idx = CV_NODE_IDX(m, n);
2421         uchar* to = newNode(idx, hash(idx));
2422         copyElem((const uchar*)CV_NODE_VAL(m, n), to, esz);
2423     }
2424 }
2425
2426 void SparseMat::create(int d, const int* _sizes, int _type)
2427 {
2428     int i;
2429     CV_Assert( _sizes && 0 < d && d <= CV_MAX_DIM );
2430     for( i = 0; i < d; i++ )
2431         CV_Assert( _sizes[i] > 0 );
2432     _type = CV_MAT_TYPE(_type);
2433     if( hdr && _type == type() && hdr->dims == d && hdr->refcount == 1 )
2434     {
2435         for( i = 0; i < d; i++ )
2436             if( _sizes[i] != hdr->size[i] )
2437                 break;
2438         if( i == d )
2439         {
2440             clear();
2441             return;
2442         }
2443     }
2444     release();
2445     flags = MAGIC_VAL | _type;
2446     hdr = new Hdr(d, _sizes, _type);
2447 }
2448
2449 void SparseMat::copyTo( SparseMat& m ) const
2450 {
2451     if( this == &m )
2452         return;
2453     if( !hdr )
2454     {
2455         m.release();
2456         return;
2457     }
2458     m.create( m.hdr->dims, m.hdr->size, type() );
2459     SparseMatConstIterator from = begin();
2460     size_t i, N = nzcount(), esz = elemSize();
2461
2462     for( i = 0; i < N; i++, ++from )
2463     {
2464         const Node* n = from.node();
2465         uchar* to = m.newNode(n->idx, n->hashval);
2466         copyElem( from.ptr, to, esz );
2467     }
2468 }
2469
2470 void SparseMat::copyTo( Mat& m ) const
2471 {
2472     CV_Assert( hdr && hdr->dims <= 2 );
2473     m.create( hdr->size[0], hdr->dims == 2 ? hdr->size[1] : 1, type() );
2474     m = Scalar(0);
2475
2476     SparseMatConstIterator from = begin();
2477     size_t i, N = nzcount(), esz = elemSize();
2478
2479     if( hdr->dims == 2 )
2480     {
2481         for( i = 0; i < N; i++, ++from )
2482         {
2483             const Node* n = from.node();
2484             uchar* to = m.data + m.step*n->idx[0] + esz*n->idx[1];
2485             copyElem( from.ptr, to, esz );
2486         }
2487     }
2488     else
2489     {
2490         for( i = 0; i < N; i++, ++from )
2491         {
2492             const Node* n = from.node();
2493             uchar* to = m.data + esz*n->idx[0];
2494             copyElem( from.ptr, to, esz );
2495         }
2496     }
2497 }
2498
2499 void SparseMat::copyTo( MatND& m ) const
2500 {
2501     CV_Assert( hdr );
2502     m.create( dims(), hdr->size, type() );
2503     m = Scalar(0);
2504
2505     SparseMatConstIterator from = begin();
2506     size_t i, N = nzcount(), esz = elemSize();
2507
2508     for( i = 0; i < N; i++, ++from )
2509     {
2510         const Node* n = from.node();
2511         copyElem( from.ptr, m.ptr(n->idx), esz);
2512     }
2513 }
2514
2515
2516 void SparseMat::convertTo( SparseMat& m, int rtype, double alpha ) const
2517 {
2518     int cn = channels();
2519     if( rtype < 0 )
2520         rtype = type();
2521     rtype = CV_MAKETYPE(rtype, cn);
2522     if( this == &m && rtype != type()  )
2523     {
2524         SparseMat temp;
2525         convertTo(temp, rtype, alpha);
2526         m = temp;
2527         return;
2528     }
2529
2530     CV_Assert(hdr != 0);
2531     m.create( m.hdr->dims, m.hdr->size, rtype );
2532
2533     SparseMatConstIterator from = begin();
2534     size_t i, N = nzcount();
2535
2536     if( alpha == 1 )
2537     {
2538         ConvertData cvtfunc = getConvertData(type(), rtype);
2539         for( i = 0; i < N; i++, ++from )
2540         {
2541             const Node* n = from.node();
2542             uchar* to = m.newNode(n->idx, n->hashval);
2543             cvtfunc( from.ptr, to, cn );
2544         }
2545     }
2546     else
2547     {
2548         ConvertScaleData cvtfunc = getConvertScaleData(type(), rtype);
2549         for( i = 0; i < N; i++, ++from )
2550         {
2551             const Node* n = from.node();
2552             uchar* to = m.newNode(n->idx, n->hashval);
2553             cvtfunc( from.ptr, to, cn, alpha, 0 );
2554         }
2555     }
2556 }
2557
2558
2559 void SparseMat::convertTo( Mat& m, int rtype, double alpha, double beta ) const
2560 {
2561     int cn = channels();
2562     if( rtype < 0 )
2563         rtype = type();
2564     rtype = CV_MAKETYPE(rtype, cn);
2565
2566     CV_Assert( hdr && hdr->dims <= 2 );
2567     m.create( hdr->size[0], hdr->dims == 2 ? hdr->size[1] : 1, type() );
2568     m = Scalar(beta);
2569
2570     SparseMatConstIterator from = begin();
2571     size_t i, N = nzcount(), esz = CV_ELEM_SIZE(rtype);
2572
2573     if( alpha == 1 && beta == 0 )
2574     {
2575         ConvertData cvtfunc = getConvertData(type(), rtype);
2576
2577         if( hdr->dims == 2 )
2578         {
2579             for( i = 0; i < N; i++, ++from )
2580             {
2581                 const Node* n = from.node();
2582                 uchar* to = m.data + m.step*n->idx[0] + esz*n->idx[1];
2583                 cvtfunc( from.ptr, to, cn );
2584             }
2585         }
2586         else
2587         {
2588             for( i = 0; i < N; i++, ++from )
2589             {
2590                 const Node* n = from.node();
2591                 uchar* to = m.data + esz*n->idx[0];
2592                 cvtfunc( from.ptr, to, cn );
2593             }
2594         }
2595     }
2596     else
2597     {
2598         ConvertScaleData cvtfunc = getConvertScaleData(type(), rtype);
2599
2600         if( hdr->dims == 2 )
2601         {
2602             for( i = 0; i < N; i++, ++from )
2603             {
2604                 const Node* n = from.node();
2605                 uchar* to = m.data + m.step*n->idx[0] + esz*n->idx[1];
2606                 cvtfunc( from.ptr, to, cn, alpha, beta );
2607             }
2608         }
2609         else
2610         {
2611             for( i = 0; i < N; i++, ++from )
2612             {
2613                 const Node* n = from.node();
2614                 uchar* to = m.data + esz*n->idx[0];
2615                 cvtfunc( from.ptr, to, cn, alpha, beta );
2616             }
2617         }
2618     }
2619 }
2620
2621 void SparseMat::convertTo( MatND& m, int rtype, double alpha, double beta ) const
2622 {
2623     int cn = channels();
2624     if( rtype < 0 )
2625         rtype = type();
2626     rtype = CV_MAKETYPE(rtype, cn);
2627
2628     CV_Assert( hdr );
2629     m.create( dims(), hdr->size, rtype );
2630     m = Scalar(beta);
2631
2632     SparseMatConstIterator from = begin();
2633     size_t i, N = nzcount();
2634
2635     if( alpha == 1 && beta == 0 )
2636     {
2637         ConvertData cvtfunc = getConvertData(type(), rtype);
2638         for( i = 0; i < N; i++, ++from )
2639         {
2640             const Node* n = from.node();
2641             uchar* to = m.ptr(n->idx);
2642             cvtfunc( from.ptr, to, cn );
2643         }
2644     }
2645     else
2646     {
2647         ConvertScaleData cvtfunc = getConvertScaleData(type(), rtype);
2648         for( i = 0; i < N; i++, ++from )
2649         {
2650             const Node* n = from.node();
2651             uchar* to = m.ptr(n->idx);
2652             cvtfunc( from.ptr, to, cn, alpha, beta );
2653         }
2654     }
2655 }
2656
2657 void SparseMat::clear()
2658 {
2659     if( hdr )
2660         hdr->clear();
2661 }
2662
2663 SparseMat::operator CvSparseMat*() const
2664 {
2665     if( !hdr )
2666         return 0;
2667     CvSparseMat* m = cvCreateSparseMat(hdr->dims, hdr->size, type());
2668
2669     SparseMatConstIterator from = begin();
2670     size_t i, N = nzcount(), esz = elemSize();
2671
2672     for( i = 0; i < N; i++, ++from )
2673     {
2674         const Node* n = from.node();
2675         uchar* to = cvPtrND(m, n->idx, 0, -2, 0);
2676         copyElem(from.ptr, to, esz);
2677     }
2678     return m;
2679 }
2680
2681 uchar* SparseMat::ptr(int i0, int i1, bool createMissing, size_t* hashval)
2682 {
2683     CV_Assert( hdr && hdr->dims == 2 );
2684     size_t h = hashval ? *hashval : hash(i0, i1);
2685     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
2686     uchar* pool = &hdr->pool[0];
2687     while( nidx != 0 )
2688     {
2689         Node* elem = (Node*)(pool + nidx);
2690         if( elem->hashval == h && elem->idx[0] == i0 && elem->idx[1] == i1 )
2691             return &value<uchar>(elem);
2692         nidx = elem->next;
2693     }
2694
2695     if( createMissing )
2696     {
2697         int idx[] = { i0, i1 };
2698         return newNode( idx, h );
2699     }
2700     return 0;
2701 }
2702
2703 uchar* SparseMat::ptr(int i0, int i1, int i2, bool createMissing, size_t* hashval)
2704 {
2705     CV_Assert( hdr && hdr->dims == 3 );
2706     size_t h = hashval ? *hashval : hash(i0, i1, i2);
2707     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
2708     uchar* pool = &hdr->pool[0];
2709     while( nidx != 0 )
2710     {
2711         Node* elem = (Node*)(pool + nidx);
2712         if( elem->hashval == h && elem->idx[0] == i0 &&
2713             elem->idx[1] == i1 && elem->idx[2] == i2 )
2714             return &value<uchar>(elem);
2715         nidx = elem->next;
2716     }
2717
2718     if( createMissing )
2719     {
2720         int idx[] = { i0, i1, i2 };
2721         return newNode( idx, h );
2722     }
2723     return 0;
2724 }
2725
2726 uchar* SparseMat::ptr(const int* idx, bool createMissing, size_t* hashval)
2727 {
2728     CV_Assert( hdr );
2729     int i, d = hdr->dims;
2730     size_t h = hashval ? *hashval : hash(idx);
2731     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
2732     uchar* pool = &hdr->pool[0];
2733     while( nidx != 0 )
2734     {
2735         Node* elem = (Node*)(pool + nidx);
2736         if( elem->hashval == h )
2737         {
2738             for( i = 0; i < d; i++ )
2739                 if( elem->idx[i] != idx[i] )
2740                     break;
2741             if( i == d )
2742                 return &value<uchar>(elem);
2743         }
2744         nidx = elem->next;
2745     }
2746
2747     return createMissing ? newNode(idx, h) : 0;
2748 }
2749
2750 void SparseMat::erase(int i0, int i1, size_t* hashval)
2751 {
2752     CV_Assert( hdr && hdr->dims == 2 );
2753     size_t h = hashval ? *hashval : hash(i0, i1);
2754     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
2755     uchar* pool = &hdr->pool[0];
2756     while( nidx != 0 )
2757     {
2758         Node* elem = (Node*)(pool + nidx);
2759         if( elem->hashval == h && elem->idx[0] == i0 && elem->idx[1] == i1 )
2760             break;
2761         previdx = nidx;
2762         nidx = elem->next;
2763     }
2764
2765     if( nidx )
2766         removeNode(hidx, nidx, previdx);
2767 }
2768
2769 void SparseMat::erase(int i0, int i1, int i2, size_t* hashval)
2770 {
2771     CV_Assert( hdr && hdr->dims == 3 );
2772     size_t h = hashval ? *hashval : hash(i0, i1, i2);
2773     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
2774     uchar* pool = &hdr->pool[0];
2775     while( nidx != 0 )
2776     {
2777         Node* elem = (Node*)(pool + nidx);
2778         if( elem->hashval == h && elem->idx[0] == i0 &&
2779             elem->idx[1] == i1 && elem->idx[2] == i2 )
2780             break;
2781         previdx = nidx;
2782         nidx = elem->next;
2783     }
2784
2785     if( nidx )
2786         removeNode(hidx, nidx, previdx);
2787 }
2788
2789 void SparseMat::erase(const int* idx, size_t* hashval)
2790 {
2791     CV_Assert( hdr );
2792     int i, d = hdr->dims;
2793     size_t h = hashval ? *hashval : hash(idx);
2794     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
2795     uchar* pool = &hdr->pool[0];
2796     while( nidx != 0 )
2797     {
2798         Node* elem = (Node*)(pool + nidx);
2799         if( elem->hashval == h )
2800         {
2801             for( i = 0; i < d; i++ )
2802                 if( elem->idx[i] != idx[i] )
2803                     break;
2804             if( i == d )
2805                 break;
2806         }
2807         previdx = nidx;
2808         nidx = elem->next;
2809     }
2810
2811     if( nidx )
2812         removeNode(hidx, nidx, previdx);
2813 }
2814
2815 void SparseMat::resizeHashTab(size_t newsize)
2816 {
2817     newsize = std::max(newsize, (size_t)8);
2818     if((newsize & (newsize-1)) != 0)
2819         newsize = 1 << cvCeil(std::log((double)newsize)/CV_LOG2);
2820
2821     size_t i, hsize = hdr->hashtab.size();
2822     vector<size_t> _newh(newsize);
2823     size_t* newh = &_newh[0];
2824     for( i = 0; i < newsize; i++ )
2825         newh[i] = 0;
2826     uchar* pool = &hdr->pool[0];
2827     for( i = 0; i < hsize; i++ )
2828     {
2829         size_t nidx = hdr->hashtab[i];
2830         while( nidx )
2831         {
2832             Node* elem = (Node*)(pool + nidx);
2833             size_t next = elem->next;
2834             size_t newhidx = elem->hashval & (newsize - 1);
2835             elem->next = newh[newhidx];
2836             newh[newhidx] = nidx;
2837             nidx = next;
2838         }
2839     }
2840     hdr->hashtab = _newh;
2841 }
2842
2843 uchar* SparseMat::newNode(const int* idx, size_t hashval)
2844 {
2845     const int HASH_MAX_FILL_FACTOR=3;
2846     assert(hdr);
2847     size_t hsize = hdr->hashtab.size();
2848     if( ++hdr->nodeCount > hsize*HASH_MAX_FILL_FACTOR )
2849     {
2850         resizeHashTab(std::max(hsize*2, (size_t)8));
2851         hsize = hdr->hashtab.size();
2852     }
2853
2854     if( !hdr->freeList )
2855     {
2856         size_t i, nsz = hdr->nodeSize, psize = hdr->pool.size(),
2857             newpsize = std::max(psize*2, 8*nsz);
2858         hdr->pool.resize(newpsize);
2859         uchar* pool = &hdr->pool[0];
2860         hdr->freeList = std::max(psize, nsz);
2861         for( i = hdr->freeList; i < newpsize - nsz; i += nsz )
2862             ((Node*)(pool + i))->next = i + nsz;
2863         ((Node*)(pool + i))->next = 0;
2864     }
2865     size_t nidx = hdr->freeList;
2866     Node* elem = (Node*)&hdr->pool[nidx];
2867     hdr->freeList = elem->next;
2868     elem->hashval = hashval;
2869     size_t hidx = hashval & (hsize - 1);
2870     elem->next = hdr->hashtab[hidx];
2871     hdr->hashtab[hidx] = nidx;
2872
2873     size_t i, d = hdr->dims;
2874     for( i = 0; i < d; i++ )
2875         elem->idx[i] = idx[i];
2876     d = elemSize();
2877     uchar* p = &value<uchar>(elem);
2878     for( i = 0; i <= d - sizeof(int); i += sizeof(int) )
2879         *(int*)(p + i) = 0;
2880     for( ; i < d; i++ )
2881         p[i] = 0;
2882
2883     return p;
2884 }
2885
2886
2887 void SparseMat::removeNode(size_t hidx, size_t nidx, size_t previdx)
2888 {
2889     Node* n = node(nidx);
2890     if( previdx )
2891     {
2892         Node* prev = node(previdx);
2893         prev->next = n->next;
2894     }
2895     else
2896         hdr->hashtab[hidx] = n->next;
2897     n->next = hdr->freeList;
2898     hdr->freeList = nidx;
2899     --hdr->nodeCount;
2900 }
2901
2902
2903 SparseMatConstIterator::SparseMatConstIterator(const SparseMat* _m)
2904 : m((SparseMat*)_m), hashidx(0), ptr(0)
2905 {
2906     if(!_m || !_m->hdr)
2907         return;
2908     SparseMat::Hdr& hdr = *m->hdr;
2909     const vector<size_t>& htab = hdr.hashtab;
2910     size_t i, hsize = htab.size();
2911     for( i = 0; i < hsize; i++ )
2912     {
2913         size_t nidx = htab[i];
2914         if( nidx )
2915         {
2916             hashidx = i;
2917             ptr = &hdr.pool[nidx] + hdr.valueOffset;
2918             return;
2919         }
2920     }
2921 }
2922
2923 SparseMatConstIterator& SparseMatConstIterator::operator ++()
2924 {
2925     if( !ptr || !m || !m->hdr )
2926         return *this;
2927     SparseMat::Hdr& hdr = *m->hdr;
2928     size_t next = ((const SparseMat::Node*)(ptr - hdr.valueOffset))->next;
2929     if( next )
2930     {
2931         ptr = &hdr.pool[next] + hdr.valueOffset;
2932         return *this;
2933     }
2934     size_t i = hashidx + 1, sz = hdr.hashtab.size();
2935     for( ; i < sz; i++ )
2936     {
2937         size_t nidx = hdr.hashtab[i];
2938         if( nidx )
2939         {
2940             hashidx = i;
2941             ptr = &hdr.pool[nidx] + hdr.valueOffset;
2942             return *this;
2943         }
2944     }
2945     hashidx = sz;
2946     ptr = 0;
2947     return *this;
2948 }
2949
2950
2951 double norm( const SparseMat& src, int normType )
2952 {
2953     SparseMatConstIterator it;
2954
2955     size_t i, N = src.nzcount();
2956     normType &= NORM_TYPE_MASK;
2957     int type = src.type();
2958     double result = 0;
2959
2960     CV_Assert( normType == NORM_INF || normType == NORM_L1 || normType == NORM_L2 );
2961
2962     if( type == CV_32F )
2963     {
2964         if( normType == NORM_INF )
2965             for( i = 0; i < N; i++, ++it )
2966                 result = std::max(result, (double)*(const float*)it.ptr);
2967         else if( normType == NORM_L1 )
2968             for( i = 0; i < N; i++, ++it )
2969                 result += std::abs(*(const float*)it.ptr);
2970         else
2971             for( i = 0; i < N; i++, ++it )
2972             {
2973                 double v = *(const float*)it.ptr;
2974                 result += v*v;
2975             }
2976     }
2977     else if( type == CV_64F )
2978     {
2979         if( normType == NORM_INF )
2980             for( i = 0; i < N; i++, ++it )
2981                 result = std::max(result, *(const double*)it.ptr);
2982         else if( normType == NORM_L1 )
2983             for( i = 0; i < N; i++, ++it )
2984                 result += std::abs(*(const double*)it.ptr);
2985         else
2986             for( i = 0; i < N; i++, ++it )
2987             {
2988                 double v = *(const double*)it.ptr;
2989                 result += v*v;
2990             }
2991     }
2992     else
2993         CV_Error( CV_StsUnsupportedFormat, "Only 32f and 64f are supported" );
2994
2995     if( normType == NORM_L2 )
2996         result = std::sqrt(result);
2997     return result;
2998 }
2999
3000 void minMaxLoc( const SparseMat& src, double* _minval, double* _maxval, int* _minidx, int* _maxidx )
3001 {
3002     SparseMatConstIterator it;
3003
3004     size_t i, N = src.nzcount(), d = src.hdr ? src.hdr->dims : 0;
3005     int type = src.type();
3006     const int *minidx = 0, *maxidx = 0;
3007
3008     if( type == CV_32F )
3009     {
3010         float minval = FLT_MAX, maxval = -FLT_MAX;
3011         for( i = 0; i < N; i++, ++it )
3012         {
3013             float v = *(const float*)it.ptr;
3014             if( v < minval )
3015             {
3016                 minval = v;
3017                 minidx = it.node()->idx;
3018             }
3019             if( v > maxval )
3020             {
3021                 maxval = v;
3022                 maxidx = it.node()->idx;
3023             }
3024         }
3025         if( _minval )
3026             *_minval = minval;
3027         if( _maxval )
3028             *_maxval = maxval;
3029     }
3030     else if( type == CV_64F )
3031     {
3032         double minval = DBL_MAX, maxval = -DBL_MAX;
3033         for( i = 0; i < N; i++, ++it )
3034         {
3035             double v = *(const double*)it.ptr;
3036             if( v < minval )
3037             {
3038                 minval = v;
3039                 minidx = it.node()->idx;
3040             }
3041             if( v > maxval )
3042             {
3043                 maxval = v;
3044                 maxidx = it.node()->idx;
3045             }
3046         }
3047         if( _minval )
3048             *_minval = minval;
3049         if( _maxval )
3050             *_maxval = maxval;
3051     }
3052     else
3053         CV_Error( CV_StsUnsupportedFormat, "Only 32f and 64f are supported" );
3054
3055     if( _minidx )
3056         for( i = 0; i < d; i++ )
3057             _minidx[i] = minidx[i];
3058     if( _maxidx )
3059         for( i = 0; i < d; i++ )
3060             _maxidx[i] = maxidx[i];
3061 }
3062
3063
3064 void normalize( const SparseMat& src, SparseMat& dst, double a, int norm_type )
3065 {
3066     double scale = 1;
3067     if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
3068     {
3069         scale = norm( src, norm_type );
3070         scale = scale > DBL_EPSILON ? a/scale : 0.;
3071     }
3072     else
3073         CV_Error( CV_StsBadArg, "Unknown/unsupported norm type" );
3074
3075     src.convertTo( dst, -1, scale );
3076 }
3077
3078
3079 }
3080
3081 /* End of file. */