modules/core/src/matrix.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 "precomp.hpp"
  44 #include "opencv2/core/gpumat.hpp"
  45 #include "opencv2/core/opengl_interop.hpp"
  46
  47 /****************************************************************************************\
  48 *                           [scaled] Identity matrix initialization                      *
  49 \****************************************************************************************/
  50
  51 namespace cv {
  52
  53 void swap( Mat& a, Mat& b )
  54 {
  55     std::swap(a.flags, b.flags);
  56     std::swap(a.dims, b.dims);
  57     std::swap(a.rows, b.rows);
  58     std::swap(a.cols, b.cols);
  59     std::swap(a.data, b.data);
  60     std::swap(a.refcount, b.refcount);
  61     std::swap(a.datastart, b.datastart);
  62     std::swap(a.dataend, b.dataend);
  63     std::swap(a.datalimit, b.datalimit);
  64     std::swap(a.allocator, b.allocator);
  65
  66     std::swap(a.size.p, b.size.p);
  67     std::swap(a.step.p, b.step.p);
  68     std::swap(a.step.buf[0], b.step.buf[0]);
  69     std::swap(a.step.buf[1], b.step.buf[1]);
  70
  71     if( a.step.p == b.step.buf )
  72     {
  73         a.step.p = a.step.buf;
  74         a.size.p = &a.rows;
  75     }
  76
  77     if( b.step.p == a.step.buf )
  78     {
  79         b.step.p = b.step.buf;
  80         b.size.p = &b.rows;
  81     }
  82 }
  83
  84
  85 static inline void setSize( Mat& m, int _dims, const int* _sz,
  86                             const size_t* _steps, bool autoSteps=false )
  87 {
  88     CV_Assert( 0 <= _dims && _dims <= CV_MAX_DIM );
  89     if( m.dims != _dims )
  90     {
  91         if( m.step.p != m.step.buf )
  92         {
  93             fastFree(m.step.p);
  94             m.step.p = m.step.buf;
  95             m.size.p = &m.rows;
  96         }
  97         if( _dims > 2 )
  98         {
  99             m.step.p = (size_t*)fastMalloc(_dims*sizeof(m.step.p[0]) + (_dims+1)*sizeof(m.size.p[0]));
 100             m.size.p = (int*)(m.step.p + _dims) + 1;
 101             m.size.p[-1] = _dims;
 102             m.rows = m.cols = -1;
 103         }
 104     }
 105
 106     m.dims = _dims;
 107     if( !_sz )
 108         return;
 109
 110     size_t esz = CV_ELEM_SIZE(m.flags), total = esz;
 111     int i;
 112     for( i = _dims-1; i >= 0; i-- )
 113     {
 114         int s = _sz[i];
 115         CV_Assert( s >= 0 );
 116         m.size.p[i] = s;
 117
 118         if( _steps )
 119             m.step.p[i] = i < _dims-1 ? _steps[i] : esz;
 120         else if( autoSteps )
 121         {
 122             m.step.p[i] = total;
 123             int64 total1 = (int64)total*s;
 124             if( (uint64)total1 != (size_t)total1 )
 125                 CV_Error( CV_StsOutOfRange, "The total matrix size does not fit to \"size_t\" type" );
 126             total = (size_t)total1;
 127         }
 128     }
 129
 130     if( _dims == 1 )
 131     {
 132         m.dims = 2;
 133         m.cols = 1;
 134         m.step[1] = esz;
 135     }
 136 }
 137
 138 static void updateContinuityFlag(Mat& m)
 139 {
 140     int i, j;
 141     for( i = 0; i < m.dims; i++ )
 142     {
 143         if( m.size[i] > 1 )
 144             break;
 145     }
 146
 147     for( j = m.dims-1; j > i; j-- )
 148     {
 149         if( m.step[j]*m.size[j] < m.step[j-1] )
 150             break;
 151     }
 152
 153     uint64 t = (uint64)m.step[0]*m.size[0];
 154     if( j <= i && t == (size_t)t )
 155         m.flags |= Mat::CONTINUOUS_FLAG;
 156     else
 157         m.flags &= ~Mat::CONTINUOUS_FLAG;
 158 }
 159
 160 static void finalizeHdr(Mat& m)
 161 {
 162     updateContinuityFlag(m);
 163     int d = m.dims;
 164     if( d > 2 )
 165         m.rows = m.cols = -1;
 166     if( m.data )
 167     {
 168         m.datalimit = m.datastart + m.size[0]*m.step[0];
 169         if( m.size[0] > 0 )
 170         {
 171             m.dataend = m.data + m.size[d-1]*m.step[d-1];
 172             for( int i = 0; i < d-1; i++ )
 173                 m.dataend += (m.size[i] - 1)*m.step[i];
 174         }
 175         else
 176             m.dataend = m.datalimit;
 177     }
 178     else
 179         m.dataend = m.datalimit = 0;
 180 }
 181
 182
 183 void Mat::create(int d, const int* _sizes, int _type)
 184 {
 185     int i;
 186     CV_Assert(0 <= d && _sizes && d <= CV_MAX_DIM && _sizes);
 187     _type = CV_MAT_TYPE(_type);
 188
 189     if( data && (d == dims || (d == 1 && dims <= 2)) && _type == type() )
 190     {
 191         if( d == 2 && rows == _sizes[0] && cols == _sizes[1] )
 192             return;
 193         for( i = 0; i < d; i++ )
 194             if( size[i] != _sizes[i] )
 195                 break;
 196         if( i == d && (d > 1 || size[1] == 1))
 197             return;
 198     }
 199
 200     release();
 201     if( d == 0 )
 202         return;
 203     flags = (_type & CV_MAT_TYPE_MASK) | MAGIC_VAL;
 204     setSize(*this, d, _sizes, 0, true);
 205
 206     if( total() > 0 )
 207     {
 208 #ifdef HAVE_TGPU
 209         if( !allocator || allocator == tegra::getAllocator() ) allocator = tegra::getAllocator(d, _sizes, _type);
 210 #endif
 211         if( !allocator )
 212         {
 213             size_t totalsize = alignSize(step.p[0]*size.p[0], (int)sizeof(*refcount));
 214             data = datastart = (uchar*)fastMalloc(totalsize + (int)sizeof(*refcount));
 215             refcount = (int*)(data + totalsize);
 216             *refcount = 1;
 217         }
 218         else
 219         {
 220 #ifdef HAVE_TGPU
 221            try
 222             {
 223                 allocator->allocate(dims, size, _type, refcount, datastart, data, step.p);
 224                 CV_Assert( step[dims-1] == (size_t)CV_ELEM_SIZE(flags) );
 225             }catch(...)
 226             {
 227                 allocator = 0;
 228                 size_t totalSize = alignSize(step.p[0]*size.p[0], (int)sizeof(*refcount));
 229                 data = datastart = (uchar*)fastMalloc(totalSize + (int)sizeof(*refcount));
 230                 refcount = (int*)(data + totalSize);
 231                 *refcount = 1;
 232             }
 233 #else
 234             allocator->allocate(dims, size, _type, refcount, datastart, data, step.p);
 235             CV_Assert( step[dims-1] == (size_t)CV_ELEM_SIZE(flags) );
 236 #endif
 237         }
 238     }
 239
 240     finalizeHdr(*this);
 241 }
 242
 243 void Mat::copySize(const Mat& m)
 244 {
 245     setSize(*this, m.dims, 0, 0);
 246     for( int i = 0; i < dims; i++ )
 247     {
 248         size[i] = m.size[i];
 249         step[i] = m.step[i];
 250     }
 251 }
 252
 253 void Mat::deallocate()
 254 {
 255     if( allocator )
 256         allocator->deallocate(refcount, datastart, data);
 257     else
 258     {
 259         CV_DbgAssert(refcount != 0);
 260         fastFree(datastart);
 261     }
 262 }
 263
 264
 265 Mat::Mat(const Mat& m, const Range& _rowRange, const Range& _colRange) : size(&rows)
 266 {
 267     initEmpty();
 268     CV_Assert( m.dims >= 2 );
 269     if( m.dims > 2 )
 270     {
 271         AutoBuffer<Range> rs(m.dims);
 272         rs[0] = _rowRange;
 273         rs[1] = _colRange;
 274         for( int i = 2; i < m.dims; i++ )
 275             rs[i] = Range::all();
 276         *this = m(rs);
 277         return;
 278     }
 279
 280     *this = m;
 281     if( _rowRange != Range::all() && _rowRange != Range(0,rows) )
 282     {
 283         CV_Assert( 0 <= _rowRange.start && _rowRange.start <= _rowRange.end && _rowRange.end <= m.rows );
 284         rows = _rowRange.size();
 285         data += step*_rowRange.start;
 286         flags |= SUBMATRIX_FLAG;
 287     }
 288
 289     if( _colRange != Range::all() && _colRange != Range(0,cols) )
 290     {
 291         CV_Assert( 0 <= _colRange.start && _colRange.start <= _colRange.end && _colRange.end <= m.cols );
 292         cols = _colRange.size();
 293         data += _colRange.start*elemSize();
 294         flags &= cols < m.cols ? ~CONTINUOUS_FLAG : -1;
 295         flags |= SUBMATRIX_FLAG;
 296     }
 297
 298     if( rows == 1 )
 299         flags |= CONTINUOUS_FLAG;
 300
 301     if( rows <= 0 || cols <= 0 )
 302     {
 303         release();
 304         rows = cols = 0;
 305     }
 306 }
 307
 308
 309 Mat::Mat(const Mat& m, const Rect& roi)
 310     : flags(m.flags), dims(2), rows(roi.height), cols(roi.width),
 311     data(m.data + roi.y*m.step[0]), refcount(m.refcount),
 312     datastart(m.datastart), dataend(m.dataend), datalimit(m.datalimit),
 313     allocator(m.allocator), size(&rows)
 314 {
 315     CV_Assert( m.dims <= 2 );
 316     flags &= roi.width < m.cols ? ~CONTINUOUS_FLAG : -1;
 317     flags |= roi.height == 1 ? CONTINUOUS_FLAG : 0;
 318
 319     size_t esz = CV_ELEM_SIZE(flags);
 320     data += roi.x*esz;
 321     CV_Assert( 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols &&
 322               0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows );
 323     if( refcount )
 324         CV_XADD(refcount, 1);
 325     if( roi.width < m.cols || roi.height < m.rows )
 326         flags |= SUBMATRIX_FLAG;
 327
 328     step[0] = m.step[0]; step[1] = esz;
 329
 330     if( rows <= 0 || cols <= 0 )
 331     {
 332         release();
 333         rows = cols = 0;
 334     }
 335 }
 336
 337
 338 Mat::Mat(int _dims, const int* _sizes, int _type, void* _data, const size_t* _steps) : size(&rows)
 339 {
 340     initEmpty();
 341     flags |= CV_MAT_TYPE(_type);
 342     data = datastart = (uchar*)_data;
 343     setSize(*this, _dims, _sizes, _steps, true);
 344     finalizeHdr(*this);
 345 }
 346
 347
 348 Mat::Mat(const Mat& m, const Range* ranges) : size(&rows)
 349 {
 350     initEmpty();
 351     int i, d = m.dims;
 352
 353     CV_Assert(ranges);
 354     for( i = 0; i < d; i++ )
 355     {
 356         Range r = ranges[i];
 357         CV_Assert( r == Range::all() || (0 <= r.start && r.start < r.end && r.end <= m.size[i]) );
 358     }
 359     *this = m;
 360     for( i = 0; i < d; i++ )
 361     {
 362         Range r = ranges[i];
 363         if( r != Range::all() && r != Range(0, size.p[i]))
 364         {
 365             size.p[i] = r.end - r.start;
 366             data += r.start*step.p[i];
 367             flags |= SUBMATRIX_FLAG;
 368         }
 369     }
 370     updateContinuityFlag(*this);
 371 }
 372
 373
 374 Mat::Mat(const CvMatND* m, bool copyData) : size(&rows)
 375 {
 376     initEmpty();
 377     if( !m )
 378         return;
 379     data = datastart = m->data.ptr;
 380     flags |= CV_MAT_TYPE(m->type);
 381     int _sizes[CV_MAX_DIM];
 382     size_t _steps[CV_MAX_DIM];
 383
 384     int i, d = m->dims;
 385     for( i = 0; i < d; i++ )
 386     {
 387         _sizes[i] = m->dim[i].size;
 388         _steps[i] = m->dim[i].step;
 389     }
 390
 391     setSize(*this, d, _sizes, _steps);
 392     finalizeHdr(*this);
 393
 394     if( copyData )
 395     {
 396         Mat temp(*this);
 397         temp.copyTo(*this);
 398     }
 399 }
 400
 401
 402 Mat Mat::diag(int d) const
 403 {
 404     CV_Assert( dims <= 2 );
 405     Mat m = *this;
 406     size_t esz = elemSize();
 407     int len;
 408
 409     if( d >= 0 )
 410     {
 411         len = std::min(cols - d, rows);
 412         m.data += esz*d;
 413     }
 414     else
 415     {
 416         len = std::min(rows + d, cols);
 417         m.data -= step[0]*d;
 418     }
 419     CV_DbgAssert( len > 0 );
 420
 421     m.size[0] = m.rows = len;
 422     m.size[1] = m.cols = 1;
 423     m.step[0] += (len > 1 ? esz : 0);
 424
 425     if( m.rows > 1 )
 426         m.flags &= ~CONTINUOUS_FLAG;
 427     else
 428         m.flags |= CONTINUOUS_FLAG;
 429
 430     if( size() != Size(1,1) )
 431         m.flags |= SUBMATRIX_FLAG;
 432
 433     return m;
 434 }
 435
 436
 437 Mat::Mat(const CvMat* m, bool copyData) : size(&rows)
 438 {
 439     initEmpty();
 440
 441     if( !m )
 442         return;
 443
 444     if( !copyData )
 445     {
 446         flags = MAGIC_VAL + (m->type & (CV_MAT_TYPE_MASK|CV_MAT_CONT_FLAG));
 447         dims = 2;
 448         rows = m->rows;
 449         cols = m->cols;
 450         data = datastart = m->data.ptr;
 451         size_t esz = CV_ELEM_SIZE(m->type), minstep = cols*esz, _step = m->step;
 452         if( _step == 0 )
 453             _step = minstep;
 454         datalimit = datastart + _step*rows;
 455         dataend = datalimit - _step + minstep;
 456         step[0] = _step; step[1] = esz;
 457     }
 458     else
 459     {
 460         data = datastart = dataend = 0;
 461         Mat(m->rows, m->cols, m->type, m->data.ptr, m->step).copyTo(*this);
 462     }
 463 }
 464
 465
 466 Mat::Mat(const IplImage* img, bool copyData) : size(&rows)
 467 {
 468     initEmpty();
 469
 470     if( !img )
 471         return;
 472
 473     dims = 2;
 474     CV_DbgAssert(CV_IS_IMAGE(img) && img->imageData != 0);
 475
 476     int imgdepth = IPL2CV_DEPTH(img->depth);
 477     size_t esz;
 478     step[0] = img->widthStep;
 479
 480     if(!img->roi)
 481     {
 482         CV_Assert(img->dataOrder == IPL_DATA_ORDER_PIXEL);
 483         flags = MAGIC_VAL + CV_MAKETYPE(imgdepth, img->nChannels);
 484         rows = img->height; cols = img->width;
 485         datastart = data = (uchar*)img->imageData;
 486         esz = CV_ELEM_SIZE(flags);
 487     }
 488     else
 489     {
 490         CV_Assert(img->dataOrder == IPL_DATA_ORDER_PIXEL || img->roi->coi != 0);
 491         bool selectedPlane = img->roi->coi && img->dataOrder == IPL_DATA_ORDER_PLANE;
 492         flags = MAGIC_VAL + CV_MAKETYPE(imgdepth, selectedPlane ? 1 : img->nChannels);
 493         rows = img->roi->height; cols = img->roi->width;
 494         esz = CV_ELEM_SIZE(flags);
 495         data = datastart = (uchar*)img->imageData +
 496             (selectedPlane ? (img->roi->coi - 1)*step*img->height : 0) +
 497             img->roi->yOffset*step[0] + img->roi->xOffset*esz;
 498     }
 499     datalimit = datastart + step.p[0]*rows;
 500     dataend = datastart + step.p[0]*(rows-1) + esz*cols;
 501     flags |= (cols*esz == step.p[0] || rows == 1 ? CONTINUOUS_FLAG : 0);
 502     step[1] = esz;
 503
 504     if( copyData )
 505     {
 506         Mat m = *this;
 507         release();
 508         if( !img->roi || !img->roi->coi ||
 509             img->dataOrder == IPL_DATA_ORDER_PLANE)
 510             m.copyTo(*this);
 511         else
 512         {
 513             int ch[] = {img->roi->coi - 1, 0};
 514             create(m.rows, m.cols, m.type());
 515             mixChannels(&m, 1, this, 1, ch, 1);
 516         }
 517     }
 518 }
 519
 520
 521 Mat::operator IplImage() const
 522 {
 523     CV_Assert( dims <= 2 );
 524     IplImage img;
 525     cvInitImageHeader(&img, size(), cvIplDepth(flags), channels());
 526     cvSetData(&img, data, (int)step[0]);
 527     return img;
 528 }
 529
 530
 531 void Mat::pop_back(size_t nelems)
 532 {
 533     CV_Assert( nelems <= (size_t)size.p[0] );
 534
 535     if( isSubmatrix() )
 536         *this = rowRange(0, size.p[0] - (int)nelems);
 537     else
 538     {
 539         size.p[0] -= (int)nelems;
 540         dataend -= nelems*step.p[0];
 541         /*if( size.p[0] <= 1 )
 542         {
 543             if( dims <= 2 )
 544                 flags |= CONTINUOUS_FLAG;
 545             else
 546                 updateContinuityFlag(*this);
 547         }*/
 548     }
 549 }
 550
 551
 552 void Mat::push_back_(const void* elem)
 553 {
 554     int r = size.p[0];
 555     if( isSubmatrix() || dataend + step.p[0] > datalimit )
 556         reserve( std::max(r + 1, (r*3+1)/2) );
 557
 558     size_t esz = elemSize();
 559     memcpy(data + r*step.p[0], elem, esz);
 560     size.p[0] = r + 1;
 561     dataend += step.p[0];
 562     if( esz < step.p[0] )
 563         flags &= ~CONTINUOUS_FLAG;
 564 }
 565
 566 void Mat::reserve(size_t nelems)
 567 {
 568     const size_t MIN_SIZE = 64;
 569
 570     CV_Assert( (int)nelems >= 0 );
 571     if( !isSubmatrix() && data + step.p[0]*nelems <= datalimit )
 572         return;
 573
 574     int r = size.p[0];
 575
 576     if( (size_t)r >= nelems )
 577         return;
 578
 579     size.p[0] = std::max((int)nelems, 1);
 580     size_t newsize = total()*elemSize();
 581
 582     if( newsize < MIN_SIZE )
 583         size.p[0] = (int)((MIN_SIZE + newsize - 1)*nelems/newsize);
 584
 585     Mat m(dims, size.p, type());
 586     size.p[0] = r;
 587     if( r > 0 )
 588     {
 589         Mat mpart = m.rowRange(0, r);
 590         copyTo(mpart);
 591     }
 592
 593     *this = m;
 594     size.p[0] = r;
 595     dataend = data + step.p[0]*r;
 596 }
 597
 598
 599 void Mat::resize(size_t nelems)
 600 {
 601     int saveRows = size.p[0];
 602     if( saveRows == (int)nelems )
 603         return;
 604     CV_Assert( (int)nelems >= 0 );
 605
 606     if( isSubmatrix() || data + step.p[0]*nelems > datalimit )
 607         reserve(nelems);
 608
 609     size.p[0] = (int)nelems;
 610     dataend += (size.p[0] - saveRows)*step.p[0];
 611
 612     //updateContinuityFlag(*this);
 613 }
 614
 615
 616 void Mat::resize(size_t nelems, const Scalar& s)
 617 {
 618     int saveRows = size.p[0];
 619     resize(nelems);
 620
 621     if( size.p[0] > saveRows )
 622     {
 623         Mat part = rowRange(saveRows, size.p[0]);
 624         part = s;
 625     }
 626 }
 627
 628 void Mat::push_back(const Mat& elems)
 629 {
 630     int r = size.p[0], delta = elems.size.p[0];
 631     if( delta == 0 )
 632         return;
 633     if( this == &elems )
 634     {
 635         Mat tmp = elems;
 636         push_back(tmp);
 637         return;
 638     }
 639     if( !data )
 640     {
 641         *this = elems.clone();
 642         return;
 643     }
 644
 645     size.p[0] = elems.size.p[0];
 646     bool eq = size == elems.size;
 647     size.p[0] = r;
 648     if( !eq )
 649         CV_Error(CV_StsUnmatchedSizes, "");
 650     if( type() != elems.type() )
 651         CV_Error(CV_StsUnmatchedFormats, "");
 652
 653     if( isSubmatrix() || dataend + step.p[0]*delta > datalimit )
 654         reserve( std::max(r + delta, (r*3+1)/2) );
 655
 656     size.p[0] += delta;
 657     dataend += step.p[0]*delta;
 658
 659     //updateContinuityFlag(*this);
 660
 661     if( isContinuous() && elems.isContinuous() )
 662         memcpy(data + r*step.p[0], elems.data, elems.total()*elems.elemSize());
 663     else
 664     {
 665         Mat part = rowRange(r, r + delta);
 666         elems.copyTo(part);
 667     }
 668 }
 669
 670
 671 Mat cvarrToMat(const CvArr* arr, bool copyData,
 672                bool /*allowND*/, int coiMode)
 673 {
 674     if( !arr )
 675         return Mat();
 676     if( CV_IS_MAT(arr) )
 677         return Mat((const CvMat*)arr, copyData );
 678     if( CV_IS_MATND(arr) )
 679         return Mat((const CvMatND*)arr, copyData );
 680     if( CV_IS_IMAGE(arr) )
 681     {
 682         const IplImage* iplimg = (const IplImage*)arr;
 683         if( coiMode == 0 && iplimg->roi && iplimg->roi->coi > 0 )
 684             CV_Error(CV_BadCOI, "COI is not supported by the function");
 685         return Mat(iplimg, copyData);
 686     }
 687     if( CV_IS_SEQ(arr) )
 688     {
 689         CvSeq* seq = (CvSeq*)arr;
 690         CV_Assert(seq->total > 0 && CV_ELEM_SIZE(seq->flags) == seq->elem_size);
 691         if(!copyData && seq->first->next == seq->first)
 692             return Mat(seq->total, 1, CV_MAT_TYPE(seq->flags), seq->first->data);
 693         Mat buf(seq->total, 1, CV_MAT_TYPE(seq->flags));
 694         cvCvtSeqToArray(seq, buf.data, CV_WHOLE_SEQ);
 695         return buf;
 696     }
 697     CV_Error(CV_StsBadArg, "Unknown array type");
 698     return Mat();
 699 }
 700
 701 void Mat::locateROI( Size& wholeSize, Point& ofs ) const
 702 {
 703     CV_Assert( dims <= 2 && step[0] > 0 );
 704     size_t esz = elemSize(), minstep;
 705     ptrdiff_t delta1 = data - datastart, delta2 = dataend - datastart;
 706
 707     if( delta1 == 0 )
 708         ofs.x = ofs.y = 0;
 709     else
 710     {
 711         ofs.y = (int)(delta1/step[0]);
 712         ofs.x = (int)((delta1 - step[0]*ofs.y)/esz);
 713         CV_DbgAssert( data == datastart + ofs.y*step[0] + ofs.x*esz );
 714     }
 715     minstep = (ofs.x + cols)*esz;
 716     wholeSize.height = (int)((delta2 - minstep)/step[0] + 1);
 717     wholeSize.height = std::max(wholeSize.height, ofs.y + rows);
 718     wholeSize.width = (int)((delta2 - step*(wholeSize.height-1))/esz);
 719     wholeSize.width = std::max(wholeSize.width, ofs.x + cols);
 720 }
 721
 722 Mat& Mat::adjustROI( int dtop, int dbottom, int dleft, int dright )
 723 {
 724     CV_Assert( dims <= 2 && step[0] > 0 );
 725     Size wholeSize; Point ofs;
 726     size_t esz = elemSize();
 727     locateROI( wholeSize, ofs );
 728     int row1 = std::max(ofs.y - dtop, 0), row2 = std::min(ofs.y + rows + dbottom, wholeSize.height);
 729     int col1 = std::max(ofs.x - dleft, 0), col2 = std::min(ofs.x + cols + dright, wholeSize.width);
 730     data += (row1 - ofs.y)*step + (col1 - ofs.x)*esz;
 731     rows = row2 - row1; cols = col2 - col1;
 732     size.p[0] = rows; size.p[1] = cols;
 733     if( esz*cols == step[0] || rows == 1 )
 734         flags |= CONTINUOUS_FLAG;
 735     else
 736         flags &= ~CONTINUOUS_FLAG;
 737     return *this;
 738 }
 739
 740 }
 741
 742 void cv::extractImageCOI(const CvArr* arr, OutputArray _ch, int coi)
 743 {
 744     Mat mat = cvarrToMat(arr, false, true, 1);
 745     _ch.create(mat.dims, mat.size, mat.depth());
 746     Mat ch = _ch.getMat();
 747     if(coi < 0)
 748     {
 749         CV_Assert( CV_IS_IMAGE(arr) );
 750         coi = cvGetImageCOI((const IplImage*)arr)-1;
 751     }
 752     CV_Assert(0 <= coi && coi < mat.channels());
 753     int _pairs[] = { coi, 0 };
 754     mixChannels( &mat, 1, &ch, 1, _pairs, 1 );
 755 }
 756
 757 void cv::insertImageCOI(InputArray _ch, CvArr* arr, int coi)
 758 {
 759     Mat ch = _ch.getMat(), mat = cvarrToMat(arr, false, true, 1);
 760     if(coi < 0)
 761     {
 762         CV_Assert( CV_IS_IMAGE(arr) );
 763         coi = cvGetImageCOI((const IplImage*)arr)-1;
 764     }
 765     CV_Assert(ch.size == mat.size && ch.depth() == mat.depth() && 0 <= coi && coi < mat.channels());
 766     int _pairs[] = { 0, coi };
 767     mixChannels( &ch, 1, &mat, 1, _pairs, 1 );
 768 }
 769
 770 namespace cv
 771 {
 772
 773 Mat Mat::reshape(int new_cn, int new_rows) const
 774 {
 775     int cn = channels();
 776     Mat hdr = *this;
 777
 778     if( dims > 2 && new_rows == 0 && new_cn != 0 && size[dims-1]*cn % new_cn == 0 )
 779     {
 780         hdr.flags = (hdr.flags & ~CV_MAT_CN_MASK) | ((new_cn-1) << CV_CN_SHIFT);
 781         hdr.step[dims-1] = CV_ELEM_SIZE(hdr.flags);
 782         hdr.size[dims-1] = hdr.size[dims-1]*cn / new_cn;
 783         return hdr;
 784     }
 785
 786     CV_Assert( dims <= 2 );
 787
 788     if( new_cn == 0 )
 789         new_cn = cn;
 790
 791     int total_width = cols * cn;
 792
 793     if( (new_cn > total_width || total_width % new_cn != 0) && new_rows == 0 )
 794         new_rows = rows * total_width / new_cn;
 795
 796     if( new_rows != 0 && new_rows != rows )
 797     {
 798         int total_size = total_width * rows;
 799         if( !isContinuous() )
 800             CV_Error( CV_BadStep,
 801             "The matrix is not continuous, thus its number of rows can not be changed" );
 802
 803         if( (unsigned)new_rows > (unsigned)total_size )
 804             CV_Error( CV_StsOutOfRange, "Bad new number of rows" );
 805
 806         total_width = total_size / new_rows;
 807
 808         if( total_width * new_rows != total_size )
 809             CV_Error( CV_StsBadArg, "The total number of matrix elements "
 810                                     "is not divisible by the new number of rows" );
 811
 812         hdr.rows = new_rows;
 813         hdr.step[0] = total_width * elemSize1();
 814     }
 815
 816     int new_width = total_width / new_cn;
 817
 818     if( new_width * new_cn != total_width )
 819         CV_Error( CV_BadNumChannels,
 820         "The total width is not divisible by the new number of channels" );
 821
 822     hdr.cols = new_width;
 823     hdr.flags = (hdr.flags & ~CV_MAT_CN_MASK) | ((new_cn-1) << CV_CN_SHIFT);
 824     hdr.step[1] = CV_ELEM_SIZE(hdr.flags);
 825     return hdr;
 826 }
 827
 828
 829 int Mat::checkVector(int _elemChannels, int _depth, bool _requireContinuous) const
 830 {
 831     return (depth() == _depth || _depth <= 0) &&
 832         (isContinuous() || !_requireContinuous) &&
 833         ((dims == 2 && (((rows == 1 || cols == 1) && channels() == _elemChannels) || (cols == _elemChannels))) ||
 834         (dims == 3 && channels() == 1 && size.p[2] == _elemChannels && (size.p[0] == 1 || size.p[1] == 1) &&
 835          (isContinuous() || step.p[1] == step.p[2]*size.p[2])))
 836     ? (int)(total()*channels()/_elemChannels) : -1;
 837 }
 838
 839
 840 void scalarToRawData(const Scalar& s, void* _buf, int type, int unroll_to)
 841 {
 842     int i, depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type);
 843     CV_Assert(cn <= 4);
 844     switch(depth)
 845     {
 846     case CV_8U:
 847         {
 848         uchar* buf = (uchar*)_buf;
 849         for(i = 0; i < cn; i++)
 850             buf[i] = saturate_cast<uchar>(s.val[i]);
 851         for(; i < unroll_to; i++)
 852             buf[i] = buf[i-cn];
 853         }
 854         break;
 855     case CV_8S:
 856         {
 857         schar* buf = (schar*)_buf;
 858         for(i = 0; i < cn; i++)
 859             buf[i] = saturate_cast<schar>(s.val[i]);
 860         for(; i < unroll_to; i++)
 861             buf[i] = buf[i-cn];
 862         }
 863         break;
 864     case CV_16U:
 865         {
 866         ushort* buf = (ushort*)_buf;
 867         for(i = 0; i < cn; i++)
 868             buf[i] = saturate_cast<ushort>(s.val[i]);
 869         for(; i < unroll_to; i++)
 870             buf[i] = buf[i-cn];
 871         }
 872         break;
 873     case CV_16S:
 874         {
 875         short* buf = (short*)_buf;
 876         for(i = 0; i < cn; i++)
 877             buf[i] = saturate_cast<short>(s.val[i]);
 878         for(; i < unroll_to; i++)
 879             buf[i] = buf[i-cn];
 880         }
 881         break;
 882     case CV_32S:
 883         {
 884         int* buf = (int*)_buf;
 885         for(i = 0; i < cn; i++)
 886             buf[i] = saturate_cast<int>(s.val[i]);
 887         for(; i < unroll_to; i++)
 888             buf[i] = buf[i-cn];
 889         }
 890         break;
 891     case CV_32F:
 892         {
 893         float* buf = (float*)_buf;
 894         for(i = 0; i < cn; i++)
 895             buf[i] = saturate_cast<float>(s.val[i]);
 896         for(; i < unroll_to; i++)
 897             buf[i] = buf[i-cn];
 898         }
 899         break;
 900     case CV_64F:
 901         {
 902         double* buf = (double*)_buf;
 903         for(i = 0; i < cn; i++)
 904             buf[i] = saturate_cast<double>(s.val[i]);
 905         for(; i < unroll_to; i++)
 906             buf[i] = buf[i-cn];
 907         break;
 908         }
 909     default:
 910         CV_Error(CV_StsUnsupportedFormat,"");
 911     }
 912 }
 913
 914
 915 /*************************************************************************************************\
 916                                         Input/Output Array
 917 \*************************************************************************************************/
 918
 919 _InputArray::_InputArray() : flags(0), obj(0) {}
 920 _InputArray::~_InputArray() {}
 921 _InputArray::_InputArray(const Mat& m) : flags(MAT), obj((void*)&m) {}
 922 _InputArray::_InputArray(const vector<Mat>& vec) : flags(STD_VECTOR_MAT), obj((void*)&vec) {}
 923 _InputArray::_InputArray(const double& val) : flags(FIXED_TYPE + FIXED_SIZE + MATX + CV_64F), obj((void*)&val), sz(Size(1,1)) {}
 924 _InputArray::_InputArray(const MatExpr& expr) : flags(FIXED_TYPE + FIXED_SIZE + EXPR), obj((void*)&expr) {}
 925 _InputArray::_InputArray(const GlBuffer& buf) : flags(FIXED_TYPE + FIXED_SIZE + OPENGL_BUFFER), obj((void*)&buf) {}
 926 _InputArray::_InputArray(const GlTexture& tex) : flags(FIXED_TYPE + FIXED_SIZE + OPENGL_TEXTURE), obj((void*)&tex) {}
 927 _InputArray::_InputArray(const gpu::GpuMat& d_mat) : flags(GPU_MAT), obj((void*)&d_mat) {}
 928
 929 Mat _InputArray::getMat(int i) const
 930 {
 931     int k = kind();
 932
 933     if( k == MAT )
 934     {
 935         const Mat* m = (const Mat*)obj;
 936         if( i < 0 )
 937             return *m;
 938         return m->row(i);
 939     }
 940
 941     if( k == EXPR )
 942     {
 943         CV_Assert( i < 0 );
 944         return (Mat)*((const MatExpr*)obj);
 945     }
 946
 947     if( k == MATX )
 948     {
 949         CV_Assert( i < 0 );
 950         return Mat(sz, flags, obj);
 951     }
 952
 953     if( k == STD_VECTOR )
 954     {
 955         CV_Assert( i < 0 );
 956         int t = CV_MAT_TYPE(flags);
 957         const vector<uchar>& v = *(const vector<uchar>*)obj;
 958
 959         return !v.empty() ? Mat(size(), t, (void*)&v[0]) : Mat();
 960     }
 961
 962     if( k == NONE )
 963         return Mat();
 964
 965     if( k == STD_VECTOR_VECTOR )
 966     {
 967         int t = type(i);
 968         const vector<vector<uchar> >& vv = *(const vector<vector<uchar> >*)obj;
 969         CV_Assert( 0 <= i && i < (int)vv.size() );
 970         const vector<uchar>& v = vv[i];
 971
 972         return !v.empty() ? Mat(size(i), t, (void*)&v[0]) : Mat();
 973     }
 974
 975     CV_Assert( k == STD_VECTOR_MAT );
 976     //if( k == STD_VECTOR_MAT )
 977     {
 978         const vector<Mat>& v = *(const vector<Mat>*)obj;
 979         CV_Assert( 0 <= i && i < (int)v.size() );
 980
 981         return v[i];
 982     }
 983 }
 984
 985
 986 void _InputArray::getMatVector(vector<Mat>& mv) const
 987 {
 988     int k = kind();
 989
 990     if( k == MAT )
 991     {
 992         const Mat& m = *(const Mat*)obj;
 993         int i, n = (int)m.size[0];
 994         mv.resize(n);
 995
 996         for( i = 0; i < n; i++ )
 997             mv[i] = m.dims == 2 ? Mat(1, m.cols, m.type(), (void*)m.ptr(i)) :
 998                 Mat(m.dims-1, &m.size[1], m.type(), (void*)m.ptr(i), &m.step[1]);
 999         return;
1000     }
1001
1002     if( k == EXPR )
1003     {
1004         Mat m = *(const MatExpr*)obj;
1005         int i, n = m.size[0];
1006         mv.resize(n);
1007
1008         for( i = 0; i < n; i++ )
1009             mv[i] = m.row(i);
1010         return;
1011     }
1012
1013     if( k == MATX )
1014     {
1015         size_t i, n = sz.height, esz = CV_ELEM_SIZE(flags);
1016         mv.resize(n);
1017
1018         for( i = 0; i < n; i++ )
1019             mv[i] = Mat(1, sz.width, CV_MAT_TYPE(flags), (uchar*)obj + esz*sz.width*i);
1020         return;
1021     }
1022
1023     if( k == STD_VECTOR )
1024     {
1025         const vector<uchar>& v = *(const vector<uchar>*)obj;
1026
1027         size_t i, n = v.size(), esz = CV_ELEM_SIZE(flags);
1028         int t = CV_MAT_DEPTH(flags), cn = CV_MAT_CN(flags);
1029         mv.resize(n);
1030
1031         for( i = 0; i < n; i++ )
1032             mv[i] = Mat(1, cn, t, (void*)(&v[0] + esz*i));
1033         return;
1034     }
1035
1036     if( k == NONE )
1037     {
1038         mv.clear();
1039         return;
1040     }
1041
1042     if( k == STD_VECTOR_VECTOR )
1043     {
1044         const vector<vector<uchar> >& vv = *(const vector<vector<uchar> >*)obj;
1045         int i, n = (int)vv.size();
1046         int t = CV_MAT_TYPE(flags);
1047         mv.resize(n);
1048
1049         for( i = 0; i < n; i++ )
1050         {
1051             const vector<uchar>& v = vv[i];
1052             mv[i] = Mat(size(i), t, (void*)&v[0]);
1053         }
1054         return;
1055     }
1056
1057     CV_Assert( k == STD_VECTOR_MAT );
1058     //if( k == STD_VECTOR_MAT )
1059     {
1060         const vector<Mat>& v = *(const vector<Mat>*)obj;
1061         mv.resize(v.size());
1062         std::copy(v.begin(), v.end(), mv.begin());
1063         return;
1064     }
1065 }
1066
1067 GlBuffer _InputArray::getGlBuffer() const
1068 {
1069     int k = kind();
1070
1071     CV_Assert(k == OPENGL_BUFFER);
1072     //if( k == OPENGL_BUFFER )
1073     {
1074         const GlBuffer* buf = (const GlBuffer*)obj;
1075         return *buf;
1076     }
1077 }
1078
1079 GlTexture _InputArray::getGlTexture() const
1080 {
1081     int k = kind();
1082
1083     CV_Assert(k == OPENGL_TEXTURE);
1084     //if( k == OPENGL_TEXTURE )
1085     {
1086         const GlTexture* tex = (const GlTexture*)obj;
1087         return *tex;
1088     }
1089 }
1090
1091 gpu::GpuMat _InputArray::getGpuMat() const
1092 {
1093     int k = kind();
1094
1095     CV_Assert(k == GPU_MAT);
1096     //if( k == GPU_MAT )
1097     {
1098         const gpu::GpuMat* d_mat = (const gpu::GpuMat*)obj;
1099         return *d_mat;
1100     }
1101 }
1102
1103 int _InputArray::kind() const
1104 {
1105     return flags & KIND_MASK;
1106 }
1107
1108 Size _InputArray::size(int i) const
1109 {
1110     int k = kind();
1111
1112     if( k == MAT )
1113     {
1114         CV_Assert( i < 0 );
1115         return ((const Mat*)obj)->size();
1116     }
1117
1118     if( k == EXPR )
1119     {
1120         CV_Assert( i < 0 );
1121         return ((const MatExpr*)obj)->size();
1122     }
1123
1124     if( k == MATX )
1125     {
1126         CV_Assert( i < 0 );
1127         return sz;
1128     }
1129
1130     if( k == STD_VECTOR )
1131     {
1132         CV_Assert( i < 0 );
1133         const vector<uchar>& v = *(const vector<uchar>*)obj;
1134         const vector<int>& iv = *(const vector<int>*)obj;
1135         size_t szb = v.size(), szi = iv.size();
1136         return szb == szi ? Size((int)szb, 1) : Size((int)(szb/CV_ELEM_SIZE(flags)), 1);
1137     }
1138
1139     if( k == NONE )
1140         return Size();
1141
1142     if( k == STD_VECTOR_VECTOR )
1143     {
1144         const vector<vector<uchar> >& vv = *(const vector<vector<uchar> >*)obj;
1145         if( i < 0 )
1146             return vv.empty() ? Size() : Size((int)vv.size(), 1);
1147         CV_Assert( i < (int)vv.size() );
1148         const vector<vector<int> >& ivv = *(const vector<vector<int> >*)obj;
1149
1150         size_t szb = vv[i].size(), szi = ivv[i].size();
1151         return szb == szi ? Size((int)szb, 1) : Size((int)(szb/CV_ELEM_SIZE(flags)), 1);
1152     }
1153
1154     if( k == STD_VECTOR_MAT )
1155     {
1156         const vector<Mat>& vv = *(const vector<Mat>*)obj;
1157         if( i < 0 )
1158             return vv.empty() ? Size() : Size((int)vv.size(), 1);
1159         CV_Assert( i < (int)vv.size() );
1160
1161         return vv[i].size();
1162     }
1163
1164     if( k == OPENGL_BUFFER )
1165     {
1166         CV_Assert( i < 0 );
1167         const GlBuffer* buf = (const GlBuffer*)obj;
1168         return buf->size();
1169     }
1170
1171     if( k == OPENGL_TEXTURE )
1172     {
1173         CV_Assert( i < 0 );
1174         const GlTexture* tex = (const GlTexture*)obj;
1175         return tex->size();
1176     }
1177
1178     CV_Assert( k == GPU_MAT );
1179     //if( k == GPU_MAT )
1180     {
1181         CV_Assert( i < 0 );
1182         const gpu::GpuMat* d_mat = (const gpu::GpuMat*)obj;
1183         return d_mat->size();
1184     }
1185 }
1186
1187 size_t _InputArray::total(int i) const
1188 {
1189     return size(i).area();
1190 }
1191
1192 int _InputArray::type(int i) const
1193 {
1194     int k = kind();
1195
1196     if( k == MAT )
1197         return ((const Mat*)obj)->type();
1198
1199     if( k == EXPR )
1200         return ((const MatExpr*)obj)->type();
1201
1202     if( k == MATX || k == STD_VECTOR || k == STD_VECTOR_VECTOR )
1203         return CV_MAT_TYPE(flags);
1204
1205     if( k == NONE )
1206         return -1;
1207
1208     if( k == STD_VECTOR_MAT )
1209     {
1210         const vector<Mat>& vv = *(const vector<Mat>*)obj;
1211         CV_Assert( i < (int)vv.size() );
1212
1213         return vv[i >= 0 ? i : 0].type();
1214     }
1215
1216     if( k == OPENGL_BUFFER )
1217         return ((const GlBuffer*)obj)->type();
1218
1219     if( k == OPENGL_TEXTURE )
1220         return ((const GlTexture*)obj)->type();
1221
1222     CV_Assert( k == GPU_MAT );
1223     //if( k == GPU_MAT )
1224         return ((const gpu::GpuMat*)obj)->type();
1225 }
1226
1227 int _InputArray::depth(int i) const
1228 {
1229     return CV_MAT_DEPTH(type(i));
1230 }
1231
1232 int _InputArray::channels(int i) const
1233 {
1234     return CV_MAT_CN(type(i));
1235 }
1236
1237 bool _InputArray::empty() const
1238 {
1239     int k = kind();
1240
1241     if( k == MAT )
1242         return ((const Mat*)obj)->empty();
1243
1244     if( k == EXPR )
1245         return false;
1246
1247     if( k == MATX )
1248         return false;
1249
1250     if( k == STD_VECTOR )
1251     {
1252         const vector<uchar>& v = *(const vector<uchar>*)obj;
1253         return v.empty();
1254     }
1255
1256     if( k == NONE )
1257         return true;
1258
1259     if( k == STD_VECTOR_VECTOR )
1260     {
1261         const vector<vector<uchar> >& vv = *(const vector<vector<uchar> >*)obj;
1262         return vv.empty();
1263     }
1264
1265     if( k == STD_VECTOR_MAT )
1266     {
1267         const vector<Mat>& vv = *(const vector<Mat>*)obj;
1268         return vv.empty();
1269     }
1270
1271     if( k == OPENGL_BUFFER )
1272         return ((const GlBuffer*)obj)->empty();
1273
1274     if( k == OPENGL_TEXTURE )
1275         return ((const GlTexture*)obj)->empty();
1276
1277     CV_Assert( k == GPU_MAT );
1278     //if( k == GPU_MAT )
1279         return ((const gpu::GpuMat*)obj)->empty();
1280 }
1281
1282
1283 _OutputArray::_OutputArray() {}
1284 _OutputArray::~_OutputArray() {}
1285 _OutputArray::_OutputArray(Mat& m) : _InputArray(m) {}
1286 _OutputArray::_OutputArray(vector<Mat>& vec) : _InputArray(vec) {}
1287 _OutputArray::_OutputArray(gpu::GpuMat& d_mat) : _InputArray(d_mat) {}
1288
1289 _OutputArray::_OutputArray(const Mat& m) : _InputArray(m) {flags |= FIXED_SIZE|FIXED_TYPE;}
1290 _OutputArray::_OutputArray(const vector<Mat>& vec) : _InputArray(vec) {flags |= FIXED_SIZE;}
1291 _OutputArray::_OutputArray(const gpu::GpuMat& d_mat) : _InputArray(d_mat) {flags |= FIXED_SIZE|FIXED_TYPE;}
1292
1293
1294 bool _OutputArray::fixedSize() const
1295 {
1296     return (flags & FIXED_SIZE) == FIXED_SIZE;
1297 }
1298
1299 bool _OutputArray::fixedType() const
1300 {
1301     return (flags & FIXED_TYPE) == FIXED_TYPE;
1302 }
1303
1304 void _OutputArray::create(Size _sz, int mtype, int i, bool allowTransposed, int fixedDepthMask) const
1305 {
1306     int k = kind();
1307     if( k == MAT && i < 0 && !allowTransposed && fixedDepthMask == 0 )
1308     {
1309         CV_Assert(!fixedSize() || ((Mat*)obj)->size.operator()() == _sz);
1310         CV_Assert(!fixedType() || ((Mat*)obj)->type() == mtype);
1311         ((Mat*)obj)->create(_sz, mtype);
1312         return;
1313     }
1314     if( k == GPU_MAT && i < 0 && !allowTransposed && fixedDepthMask == 0 )
1315     {
1316         CV_Assert(!fixedSize() || ((gpu::GpuMat*)obj)->size() == _sz);
1317         CV_Assert(!fixedType() || ((gpu::GpuMat*)obj)->type() == mtype);
1318         ((gpu::GpuMat*)obj)->create(_sz, mtype);
1319         return;
1320     }
1321     int sizes[] = {_sz.height, _sz.width};
1322     create(2, sizes, mtype, i, allowTransposed, fixedDepthMask);
1323 }
1324
1325 void _OutputArray::create(int rows, int cols, int mtype, int i, bool allowTransposed, int fixedDepthMask) const
1326 {
1327     int k = kind();
1328     if( k == MAT && i < 0 && !allowTransposed && fixedDepthMask == 0 )
1329     {
1330         CV_Assert(!fixedSize() || ((Mat*)obj)->size.operator()() == Size(cols, rows));
1331         CV_Assert(!fixedType() || ((Mat*)obj)->type() == mtype);
1332         ((Mat*)obj)->create(rows, cols, mtype);
1333         return;
1334     }
1335     if( k == GPU_MAT && i < 0 && !allowTransposed && fixedDepthMask == 0 )
1336     {
1337         CV_Assert(!fixedSize() || ((gpu::GpuMat*)obj)->size() == Size(cols, rows));
1338         CV_Assert(!fixedType() || ((gpu::GpuMat*)obj)->type() == mtype);
1339         ((gpu::GpuMat*)obj)->create(rows, cols, mtype);
1340         return;
1341     }
1342     int sizes[] = {rows, cols};
1343     create(2, sizes, mtype, i, allowTransposed, fixedDepthMask);
1344 }
1345
1346 void _OutputArray::create(int dims, const int* sizes, int mtype, int i, bool allowTransposed, int fixedDepthMask) const
1347 {
1348     int k = kind();
1349     mtype = CV_MAT_TYPE(mtype);
1350
1351     if( k == MAT )
1352     {
1353         CV_Assert( i < 0 );
1354         Mat& m = *(Mat*)obj;
1355         if( allowTransposed )
1356         {
1357             if( !m.isContinuous() )
1358             {
1359                 CV_Assert(!fixedType() && !fixedSize());
1360                 m.release();
1361             }
1362
1363             if( dims == 2 && m.dims == 2 && m.data &&
1364                 m.type() == mtype && m.rows == sizes[1] && m.cols == sizes[0] )
1365                 return;
1366         }
1367
1368         if(fixedType())
1369         {
1370             if(CV_MAT_CN(mtype) == m.channels() && ((1 << CV_MAT_TYPE(flags)) & fixedDepthMask) != 0 )
1371                 mtype = m.type();
1372             else
1373                 CV_Assert(CV_MAT_TYPE(mtype) == m.type());
1374         }
1375         if(fixedSize())
1376         {
1377             CV_Assert(m.dims == dims);
1378             for(int j = 0; j < dims; ++j)
1379                 CV_Assert(m.size[j] == sizes[j]);
1380         }
1381         m.create(dims, sizes, mtype);
1382         return;
1383     }
1384
1385     if( k == MATX )
1386     {
1387         CV_Assert( i < 0 );
1388         int type0 = CV_MAT_TYPE(flags);
1389         CV_Assert( mtype == type0 || (CV_MAT_CN(mtype) == 1 && ((1 << type0) & fixedDepthMask) != 0) );
1390         CV_Assert( dims == 2 && ((sizes[0] == sz.height && sizes[1] == sz.width) ||
1391                                  (allowTransposed && sizes[0] == sz.width && sizes[1] == sz.height)));
1392         return;
1393     }
1394
1395     if( k == STD_VECTOR || k == STD_VECTOR_VECTOR )
1396     {
1397         CV_Assert( dims == 2 && (sizes[0] == 1 || sizes[1] == 1 || sizes[0]*sizes[1] == 0) );
1398         size_t len = sizes[0]*sizes[1] > 0 ? sizes[0] + sizes[1] - 1 : 0;
1399         vector<uchar>* v = (vector<uchar>*)obj;
1400
1401         if( k == STD_VECTOR_VECTOR )
1402         {
1403             vector<vector<uchar> >& vv = *(vector<vector<uchar> >*)obj;
1404             if( i < 0 )
1405             {
1406                 CV_Assert(!fixedSize() || len == vv.size());
1407                 vv.resize(len);
1408                 return;
1409             }
1410             CV_Assert( i < (int)vv.size() );
1411             v = &vv[i];
1412         }
1413         else
1414             CV_Assert( i < 0 );
1415
1416         int type0 = CV_MAT_TYPE(flags);
1417         CV_Assert( mtype == type0 || (CV_MAT_CN(mtype) == CV_MAT_CN(type0) && ((1 << type0) & fixedDepthMask) != 0) );
1418
1419         int esz = CV_ELEM_SIZE(type0);
1420         CV_Assert(!fixedSize() || len == ((vector<uchar>*)v)->size() / esz);
1421         switch( esz )
1422         {
1423         case 1:
1424             ((vector<uchar>*)v)->resize(len);
1425             break;
1426         case 2:
1427             ((vector<Vec2b>*)v)->resize(len);
1428             break;
1429         case 3:
1430             ((vector<Vec3b>*)v)->resize(len);
1431             break;
1432         case 4:
1433             ((vector<int>*)v)->resize(len);
1434             break;
1435         case 6:
1436             ((vector<Vec3s>*)v)->resize(len);
1437             break;
1438         case 8:
1439             ((vector<Vec2i>*)v)->resize(len);
1440             break;
1441         case 12:
1442             ((vector<Vec3i>*)v)->resize(len);
1443             break;
1444         case 16:
1445             ((vector<Vec4i>*)v)->resize(len);
1446             break;
1447         case 24:
1448             ((vector<Vec6i>*)v)->resize(len);
1449             break;
1450         case 32:
1451             ((vector<Vec8i>*)v)->resize(len);
1452             break;
1453         case 36:
1454             ((vector<Vec<int, 9> >*)v)->resize(len);
1455             break;
1456         case 48:
1457             ((vector<Vec<int, 12> >*)v)->resize(len);
1458             break;
1459         case 64:
1460             ((vector<Vec<int, 16> >*)v)->resize(len);
1461             break;
1462         case 128:
1463             ((vector<Vec<int, 32> >*)v)->resize(len);
1464             break;
1465         case 256:
1466             ((vector<Vec<int, 64> >*)v)->resize(len);
1467             break;
1468         case 512:
1469             ((vector<Vec<int, 128> >*)v)->resize(len);
1470             break;
1471         default:
1472             CV_Error_(CV_StsBadArg, ("Vectors with element size %d are not supported. Please, modify OutputArray::create()\n", esz));
1473         }
1474         return;
1475     }
1476
1477     if( k == NONE )
1478     {
1479         CV_Error(CV_StsNullPtr, "create() called for the missing output array" );
1480         return;
1481     }
1482
1483     CV_Assert( k == STD_VECTOR_MAT );
1484     //if( k == STD_VECTOR_MAT )
1485     {
1486         vector<Mat>& v = *(vector<Mat>*)obj;
1487
1488         if( i < 0 )
1489         {
1490             CV_Assert( dims == 2 && (sizes[0] == 1 || sizes[1] == 1 || sizes[0]*sizes[1] == 0) );
1491             size_t len = sizes[0]*sizes[1] > 0 ? sizes[0] + sizes[1] - 1 : 0, len0 = v.size();
1492
1493             CV_Assert(!fixedSize() || len == len0);
1494             v.resize(len);
1495             if( fixedType() )
1496             {
1497                 int _type = CV_MAT_TYPE(flags);
1498                 for( size_t j = len0; j < len; j++ )
1499                 {
1500                     if( v[i].type() == _type )
1501                         continue;
1502                     CV_Assert( v[i].empty() );
1503                     v[i].flags = (v[i].flags & ~CV_MAT_TYPE_MASK) | _type;
1504                 }
1505             }
1506             return;
1507         }
1508
1509         CV_Assert( i < (int)v.size() );
1510         Mat& m = v[i];
1511
1512         if( allowTransposed )
1513         {
1514             if( !m.isContinuous() )
1515             {
1516                 CV_Assert(!fixedType() && !fixedSize());
1517                 m.release();
1518             }
1519
1520             if( dims == 2 && m.dims == 2 && m.data &&
1521                 m.type() == mtype && m.rows == sizes[1] && m.cols == sizes[0] )
1522                 return;
1523         }
1524
1525         if(fixedType())
1526         {
1527             if(CV_MAT_CN(mtype) == m.channels() && ((1 << CV_MAT_TYPE(flags)) & fixedDepthMask) != 0 )
1528                 mtype = m.type();
1529             else
1530                 CV_Assert(!fixedType() || (CV_MAT_CN(mtype) == m.channels() && ((1 << CV_MAT_TYPE(flags)) & fixedDepthMask) != 0));
1531         }
1532         if(fixedSize())
1533         {
1534             CV_Assert(m.dims == dims);
1535             for(int j = 0; j < dims; ++j)
1536                 CV_Assert(m.size[j] == sizes[j]);
1537         }
1538
1539         m.create(dims, sizes, mtype);
1540     }
1541 }
1542
1543 void _OutputArray::release() const
1544 {
1545     CV_Assert(!fixedSize());
1546
1547     int k = kind();
1548
1549     if( k == MAT )
1550     {
1551         ((Mat*)obj)->release();
1552         return;
1553     }
1554
1555     if( k == GPU_MAT )
1556     {
1557         ((gpu::GpuMat*)obj)->release();
1558         return;
1559     }
1560
1561     if( k == NONE )
1562         return;
1563
1564     if( k == STD_VECTOR )
1565     {
1566         create(Size(), CV_MAT_TYPE(flags));
1567         return;
1568     }
1569
1570     if( k == STD_VECTOR_VECTOR )
1571     {
1572         ((vector<vector<uchar> >*)obj)->clear();
1573         return;
1574     }
1575
1576     CV_Assert( k == STD_VECTOR_MAT );
1577     //if( k == STD_VECTOR_MAT )
1578     {
1579         ((vector<Mat>*)obj)->clear();
1580     }
1581 }
1582
1583 void _OutputArray::clear() const
1584 {
1585     int k = kind();
1586
1587     if( k == MAT )
1588     {
1589         CV_Assert(!fixedSize());
1590         ((Mat*)obj)->resize(0);
1591         return;
1592     }
1593
1594     release();
1595 }
1596
1597 bool _OutputArray::needed() const
1598 {
1599     return kind() != NONE;
1600 }
1601
1602 Mat& _OutputArray::getMatRef(int i) const
1603 {
1604     int k = kind();
1605     if( i < 0 )
1606     {
1607         CV_Assert( k == MAT );
1608         return *(Mat*)obj;
1609     }
1610     else
1611     {
1612         CV_Assert( k == STD_VECTOR_MAT );
1613         vector<Mat>& v = *(vector<Mat>*)obj;
1614         CV_Assert( i < (int)v.size() );
1615         return v[i];
1616     }
1617 }
1618
1619 gpu::GpuMat& _OutputArray::getGpuMatRef() const
1620 {
1621     int k = kind();
1622     CV_Assert( k == GPU_MAT );
1623     return *(gpu::GpuMat*)obj;
1624 }
1625
1626 static _OutputArray _none;
1627 OutputArray noArray() { return _none; }
1628
1629 }
1630
1631 /*************************************************************************************************\
1632                                         Matrix Operations
1633 \*************************************************************************************************/
1634
1635 void cv::hconcat(const Mat* src, size_t nsrc, OutputArray _dst)
1636 {
1637     if( nsrc == 0 || !src )
1638     {
1639         _dst.release();
1640         return;
1641     }
1642
1643     int totalCols = 0, cols = 0;
1644     size_t i;
1645     for( i = 0; i < nsrc; i++ )
1646     {
1647         CV_Assert( !src[i].empty() && src[i].dims <= 2 &&
1648                    src[i].rows == src[0].rows &&
1649                    src[i].type() == src[0].type());
1650         totalCols += src[i].cols;
1651     }
1652     _dst.create( src[0].rows, totalCols, src[0].type());
1653     Mat dst = _dst.getMat();
1654     for( i = 0; i < nsrc; i++ )
1655     {
1656         Mat dpart = dst(Rect(cols, 0, src[i].cols, src[i].rows));
1657         src[i].copyTo(dpart);
1658         cols += src[i].cols;
1659     }
1660 }
1661
1662 void cv::hconcat(InputArray src1, InputArray src2, OutputArray dst)
1663 {
1664     Mat src[] = {src1.getMat(), src2.getMat()};
1665     hconcat(src, 2, dst);
1666 }
1667
1668 void cv::hconcat(InputArray _src, OutputArray dst)
1669 {
1670     vector<Mat> src;
1671     _src.getMatVector(src);
1672     hconcat(!src.empty() ? &src[0] : 0, src.size(), dst);
1673 }
1674
1675 void cv::vconcat(const Mat* src, size_t nsrc, OutputArray _dst)
1676 {
1677     if( nsrc == 0 || !src )
1678     {
1679         _dst.release();
1680         return;
1681     }
1682
1683     int totalRows = 0, rows = 0;
1684     size_t i;
1685     for( i = 0; i < nsrc; i++ )
1686     {
1687         CV_Assert( !src[i].empty() && src[i].dims <= 2 &&
1688                   src[i].cols == src[0].cols &&
1689                   src[i].type() == src[0].type());
1690         totalRows += src[i].rows;
1691     }
1692     _dst.create( totalRows, src[0].cols, src[0].type());
1693     Mat dst = _dst.getMat();
1694     for( i = 0; i < nsrc; i++ )
1695     {
1696         Mat dpart(dst, Rect(0, rows, src[i].cols, src[i].rows));
1697         src[i].copyTo(dpart);
1698         rows += src[i].rows;
1699     }
1700 }
1701
1702 void cv::vconcat(InputArray src1, InputArray src2, OutputArray dst)
1703 {
1704     Mat src[] = {src1.getMat(), src2.getMat()};
1705     vconcat(src, 2, dst);
1706 }
1707
1708 void cv::vconcat(InputArray _src, OutputArray dst)
1709 {
1710     vector<Mat> src;
1711     _src.getMatVector(src);
1712     vconcat(!src.empty() ? &src[0] : 0, src.size(), dst);
1713 }
1714
1715 //////////////////////////////////////// set identity ////////////////////////////////////////////
1716 void cv::setIdentity( InputOutputArray _m, const Scalar& s )
1717 {
1718     Mat m = _m.getMat();
1719     CV_Assert( m.dims <= 2 );
1720     int i, j, rows = m.rows, cols = m.cols, type = m.type();
1721
1722     if( type == CV_32FC1 )
1723     {
1724         float* data = (float*)m.data;
1725         float val = (float)s[0];
1726         size_t step = m.step/sizeof(data[0]);
1727
1728         for( i = 0; i < rows; i++, data += step )
1729         {
1730             for( j = 0; j < cols; j++ )
1731                 data[j] = 0;
1732             if( i < cols )
1733                 data[i] = val;
1734         }
1735     }
1736     else if( type == CV_64FC1 )
1737     {
1738         double* data = (double*)m.data;
1739         double val = s[0];
1740         size_t step = m.step/sizeof(data[0]);
1741
1742         for( i = 0; i < rows; i++, data += step )
1743         {
1744             for( j = 0; j < cols; j++ )
1745                 data[j] = j == i ? val : 0;
1746         }
1747     }
1748     else
1749     {
1750         m = Scalar(0);
1751         m.diag() = s;
1752     }
1753 }
1754
1755 //////////////////////////////////////////// trace ///////////////////////////////////////////
1756
1757 cv::Scalar cv::trace( InputArray _m )
1758 {
1759     Mat m = _m.getMat();
1760     CV_Assert( m.dims <= 2 );
1761     int i, type = m.type();
1762     int nm = std::min(m.rows, m.cols);
1763
1764     if( type == CV_32FC1 )
1765     {
1766         const float* ptr = (const float*)m.data;
1767         size_t step = m.step/sizeof(ptr[0]) + 1;
1768         double _s = 0;
1769         for( i = 0; i < nm; i++ )
1770             _s += ptr[i*step];
1771         return _s;
1772     }
1773
1774     if( type == CV_64FC1 )
1775     {
1776         const double* ptr = (const double*)m.data;
1777         size_t step = m.step/sizeof(ptr[0]) + 1;
1778         double _s = 0;
1779         for( i = 0; i < nm; i++ )
1780             _s += ptr[i*step];
1781         return _s;
1782     }
1783
1784     return cv::sum(m.diag());
1785 }
1786
1787 ////////////////////////////////////// transpose /////////////////////////////////////////
1788
1789 namespace cv
1790 {
1791
1792 template<typename T> static void
1793 transpose_( const uchar* src, size_t sstep, uchar* dst, size_t dstep, Size sz )
1794 {
1795     int i=0, j, m = sz.width, n = sz.height;
1796
1797     #if CV_ENABLE_UNROLLED
1798     for(; i <= m - 4; i += 4 )
1799     {
1800         T* d0 = (T*)(dst + dstep*i);
1801         T* d1 = (T*)(dst + dstep*(i+1));
1802         T* d2 = (T*)(dst + dstep*(i+2));
1803         T* d3 = (T*)(dst + dstep*(i+3));
1804
1805         for( j = 0; j <= n - 4; j += 4 )
1806         {
1807             const T* s0 = (const T*)(src + i*sizeof(T) + sstep*j);
1808             const T* s1 = (const T*)(src + i*sizeof(T) + sstep*(j+1));
1809             const T* s2 = (const T*)(src + i*sizeof(T) + sstep*(j+2));
1810             const T* s3 = (const T*)(src + i*sizeof(T) + sstep*(j+3));
1811
1812             d0[j] = s0[0]; d0[j+1] = s1[0]; d0[j+2] = s2[0]; d0[j+3] = s3[0];
1813             d1[j] = s0[1]; d1[j+1] = s1[1]; d1[j+2] = s2[1]; d1[j+3] = s3[1];
1814             d2[j] = s0[2]; d2[j+1] = s1[2]; d2[j+2] = s2[2]; d2[j+3] = s3[2];
1815             d3[j] = s0[3]; d3[j+1] = s1[3]; d3[j+2] = s2[3]; d3[j+3] = s3[3];
1816         }
1817
1818         for( ; j < n; j++ )
1819         {
1820             const T* s0 = (const T*)(src + i*sizeof(T) + j*sstep);
1821             d0[j] = s0[0]; d1[j] = s0[1]; d2[j] = s0[2]; d3[j] = s0[3];
1822         }
1823     }
1824     #endif
1825     for( ; i < m; i++ )
1826     {
1827         T* d0 = (T*)(dst + dstep*i);
1828         j = 0;
1829         #if CV_ENABLE_UNROLLED
1830         for(; j <= n - 4; j += 4 )
1831         {
1832             const T* s0 = (const T*)(src + i*sizeof(T) + sstep*j);
1833             const T* s1 = (const T*)(src + i*sizeof(T) + sstep*(j+1));
1834             const T* s2 = (const T*)(src + i*sizeof(T) + sstep*(j+2));
1835             const T* s3 = (const T*)(src + i*sizeof(T) + sstep*(j+3));
1836
1837             d0[j] = s0[0]; d0[j+1] = s1[0]; d0[j+2] = s2[0]; d0[j+3] = s3[0];
1838         }
1839         #endif
1840         for( ; j < n; j++ )
1841         {
1842             const T* s0 = (const T*)(src + i*sizeof(T) + j*sstep);
1843             d0[j] = s0[0];
1844         }
1845     }
1846 }
1847
1848 template<typename T> static void
1849 transposeI_( uchar* data, size_t step, int n )
1850 {
1851     int i, j;
1852     for( i = 0; i < n; i++ )
1853     {
1854         T* row = (T*)(data + step*i);
1855         uchar* data1 = data + i*sizeof(T);
1856         for( j = i+1; j < n; j++ )
1857             std::swap( row[j], *(T*)(data1 + step*j) );
1858     }
1859 }
1860
1861 typedef void (*TransposeFunc)( const uchar* src, size_t sstep, uchar* dst, size_t dstep, Size sz );
1862 typedef void (*TransposeInplaceFunc)( uchar* data, size_t step, int n );
1863
1864 #define DEF_TRANSPOSE_FUNC(suffix, type) \
1865 static void transpose_##suffix( const uchar* src, size_t sstep, uchar* dst, size_t dstep, Size sz ) \
1866 { transpose_<type>(src, sstep, dst, dstep, sz); } \
1867 \
1868 static void transposeI_##suffix( uchar* data, size_t step, int n ) \
1869 { transposeI_<type>(data, step, n); }
1870
1871 DEF_TRANSPOSE_FUNC(8u, uchar)
1872 DEF_TRANSPOSE_FUNC(16u, ushort)
1873 DEF_TRANSPOSE_FUNC(8uC3, Vec3b)
1874 DEF_TRANSPOSE_FUNC(32s, int)
1875 DEF_TRANSPOSE_FUNC(16uC3, Vec3s)
1876 DEF_TRANSPOSE_FUNC(32sC2, Vec2i)
1877 DEF_TRANSPOSE_FUNC(32sC3, Vec3i)
1878 DEF_TRANSPOSE_FUNC(32sC4, Vec4i)
1879 DEF_TRANSPOSE_FUNC(32sC6, Vec6i)
1880 DEF_TRANSPOSE_FUNC(32sC8, Vec8i)
1881
1882 static TransposeFunc transposeTab[] =
1883 {
1884     0, transpose_8u, transpose_16u, transpose_8uC3, transpose_32s, 0, transpose_16uC3, 0,
1885     transpose_32sC2, 0, 0, 0, transpose_32sC3, 0, 0, 0, transpose_32sC4,
1886     0, 0, 0, 0, 0, 0, 0, transpose_32sC6, 0, 0, 0, 0, 0, 0, 0, transpose_32sC8
1887 };
1888
1889 static TransposeInplaceFunc transposeInplaceTab[] =
1890 {
1891     0, transposeI_8u, transposeI_16u, transposeI_8uC3, transposeI_32s, 0, transposeI_16uC3, 0,
1892     transposeI_32sC2, 0, 0, 0, transposeI_32sC3, 0, 0, 0, transposeI_32sC4,
1893     0, 0, 0, 0, 0, 0, 0, transposeI_32sC6, 0, 0, 0, 0, 0, 0, 0, transposeI_32sC8
1894 };
1895
1896 }
1897
1898 void cv::transpose( InputArray _src, OutputArray _dst )
1899 {
1900     Mat src = _src.getMat();
1901     size_t esz = src.elemSize();
1902     CV_Assert( src.dims <= 2 && esz <= (size_t)32 );
1903
1904     _dst.create(src.cols, src.rows, src.type());
1905     Mat dst = _dst.getMat();
1906
1907     if( dst.data == src.data )
1908     {
1909         TransposeInplaceFunc func = transposeInplaceTab[esz];
1910         CV_Assert( func != 0 );
1911         func( dst.data, dst.step, dst.rows );
1912     }
1913     else
1914     {
1915         TransposeFunc func = transposeTab[esz];
1916         CV_Assert( func != 0 );
1917         func( src.data, src.step, dst.data, dst.step, src.size() );
1918     }
1919 }
1920
1921
1922 void cv::completeSymm( InputOutputArray _m, bool LtoR )
1923 {
1924     Mat m = _m.getMat();
1925     CV_Assert( m.dims <= 2 );
1926
1927     int i, j, nrows = m.rows, type = m.type();
1928     int j0 = 0, j1 = nrows;
1929     CV_Assert( m.rows == m.cols );
1930
1931     if( type == CV_32FC1 || type == CV_32SC1 )
1932     {
1933         int* data = (int*)m.data;
1934         size_t step = m.step/sizeof(data[0]);
1935         for( i = 0; i < nrows; i++ )
1936         {
1937             if( !LtoR ) j1 = i; else j0 = i+1;
1938             for( j = j0; j < j1; j++ )
1939                 data[i*step + j] = data[j*step + i];
1940         }
1941     }
1942     else if( type == CV_64FC1 )
1943     {
1944         double* data = (double*)m.data;
1945         size_t step = m.step/sizeof(data[0]);
1946         for( i = 0; i < nrows; i++ )
1947         {
1948             if( !LtoR ) j1 = i; else j0 = i+1;
1949             for( j = j0; j < j1; j++ )
1950                 data[i*step + j] = data[j*step + i];
1951         }
1952     }
1953     else
1954         CV_Error( CV_StsUnsupportedFormat, "" );
1955 }
1956
1957
1958 cv::Mat cv::Mat::cross(InputArray _m) const
1959 {
1960     Mat m = _m.getMat();
1961     int tp = type(), d = CV_MAT_DEPTH(tp);
1962     CV_Assert( dims <= 2 && m.dims <= 2 && size() == m.size() && tp == m.type() &&
1963         ((rows == 3 && cols == 1) || (cols*channels() == 3 && rows == 1)));
1964     Mat result(rows, cols, tp);
1965
1966     if( d == CV_32F )
1967     {
1968         const float *a = (const float*)data, *b = (const float*)m.data;
1969         float* c = (float*)result.data;
1970         size_t lda = rows > 1 ? step/sizeof(a[0]) : 1;
1971         size_t ldb = rows > 1 ? m.step/sizeof(b[0]) : 1;
1972
1973         c[0] = a[lda] * b[ldb*2] - a[lda*2] * b[ldb];
1974         c[1] = a[lda*2] * b[0] - a[0] * b[ldb*2];
1975         c[2] = a[0] * b[ldb] - a[lda] * b[0];
1976     }
1977     else if( d == CV_64F )
1978     {
1979         const double *a = (const double*)data, *b = (const double*)m.data;
1980         double* c = (double*)result.data;
1981         size_t lda = rows > 1 ? step/sizeof(a[0]) : 1;
1982         size_t ldb = rows > 1 ? m.step/sizeof(b[0]) : 1;
1983
1984         c[0] = a[lda] * b[ldb*2] - a[lda*2] * b[ldb];
1985         c[1] = a[lda*2] * b[0] - a[0] * b[ldb*2];
1986         c[2] = a[0] * b[ldb] - a[lda] * b[0];
1987     }
1988
1989     return result;
1990 }
1991
1992
1993 ////////////////////////////////////////// reduce ////////////////////////////////////////////
1994
1995 namespace cv
1996 {
1997
1998 template<typename T, typename ST, class Op> static void
1999 reduceR_( const Mat& srcmat, Mat& dstmat )
2000 {
2001     typedef typename Op::rtype WT;
2002     Size size = srcmat.size();
2003     size.width *= srcmat.channels();
2004     AutoBuffer<WT> buffer(size.width);
2005     WT* buf = buffer;
2006     ST* dst = (ST*)dstmat.data;
2007     const T* src = (const T*)srcmat.data;
2008     size_t srcstep = srcmat.step/sizeof(src[0]);
2009     int i;
2010     Op op;
2011
2012     for( i = 0; i < size.width; i++ )
2013         buf[i] = src[i];
2014
2015     for( ; --size.height; )
2016     {
2017         src += srcstep;
2018         i = 0;
2019         #if CV_ENABLE_UNROLLED
2020         for(; i <= size.width - 4; i += 4 )
2021         {
2022             WT s0, s1;
2023             s0 = op(buf[i], (WT)src[i]);
2024             s1 = op(buf[i+1], (WT)src[i+1]);
2025             buf[i] = s0; buf[i+1] = s1;
2026
2027             s0 = op(buf[i+2], (WT)src[i+2]);
2028             s1 = op(buf[i+3], (WT)src[i+3]);
2029             buf[i+2] = s0; buf[i+3] = s1;
2030         }
2031         #endif
2032         for( ; i < size.width; i++ )
2033             buf[i] = op(buf[i], (WT)src[i]);
2034     }
2035
2036     for( i = 0; i < size.width; i++ )
2037         dst[i] = (ST)buf[i];
2038 }
2039
2040
2041 template<typename T, typename ST, class Op> static void
2042 reduceC_( const Mat& srcmat, Mat& dstmat )
2043 {
2044     typedef typename Op::rtype WT;
2045     Size size = srcmat.size();
2046     int i, k, cn = srcmat.channels();
2047     size.width *= cn;
2048     Op op;
2049
2050     for( int y = 0; y < size.height; y++ )
2051     {
2052         const T* src = (const T*)(srcmat.data + srcmat.step*y);
2053         ST* dst = (ST*)(dstmat.data + dstmat.step*y);
2054         if( size.width == cn )
2055             for( k = 0; k < cn; k++ )
2056                 dst[k] = src[k];
2057         else
2058         {
2059             for( k = 0; k < cn; k++ )
2060             {
2061                 WT a0 = src[k], a1 = src[k+cn];
2062                 for( i = 2*cn; i <= size.width - 4*cn; i += 4*cn )
2063                 {
2064                     a0 = op(a0, (WT)src[i+k]);
2065                     a1 = op(a1, (WT)src[i+k+cn]);
2066                     a0 = op(a0, (WT)src[i+k+cn*2]);
2067                     a1 = op(a1, (WT)src[i+k+cn*3]);
2068                 }
2069
2070                 for( ; i < size.width; i += cn )
2071                 {
2072                     a0 = op(a0, (WT)src[i+k]);
2073                 }
2074                 a0 = op(a0, a1);
2075               dst[k] = (ST)a0;
2076             }
2077         }
2078     }
2079 }
2080
2081 typedef void (*ReduceFunc)( const Mat& src, Mat& dst );
2082
2083 }
2084
2085 #define reduceSumR8u32s  reduceR_<uchar, int,   OpAdd<int> >
2086 #define reduceSumR8u32f  reduceR_<uchar, float, OpAdd<int> >
2087 #define reduceSumR8u64f  reduceR_<uchar, double,OpAdd<int> >
2088 #define reduceSumR16u32f reduceR_<ushort,float, OpAdd<float> >
2089 #define reduceSumR16u64f reduceR_<ushort,double,OpAdd<double> >
2090 #define reduceSumR16s32f reduceR_<short, float, OpAdd<float> >
2091 #define reduceSumR16s64f reduceR_<short, double,OpAdd<double> >
2092 #define reduceSumR32f32f reduceR_<float, float, OpAdd<float> >
2093 #define reduceSumR32f64f reduceR_<float, double,OpAdd<double> >
2094 #define reduceSumR64f64f reduceR_<double,double,OpAdd<double> >
2095
2096 #define reduceMaxR8u  reduceR_<uchar, uchar, OpMax<uchar> >
2097 #define reduceMaxR16u reduceR_<ushort,ushort,OpMax<ushort> >
2098 #define reduceMaxR16s reduceR_<short, short, OpMax<short> >
2099 #define reduceMaxR32f reduceR_<float, float, OpMax<float> >
2100 #define reduceMaxR64f reduceR_<double,double,OpMax<double> >
2101
2102 #define reduceMinR8u  reduceR_<uchar, uchar, OpMin<uchar> >
2103 #define reduceMinR16u reduceR_<ushort,ushort,OpMin<ushort> >
2104 #define reduceMinR16s reduceR_<short, short, OpMin<short> >
2105 #define reduceMinR32f reduceR_<float, float, OpMin<float> >
2106 #define reduceMinR64f reduceR_<double,double,OpMin<double> >
2107
2108 #define reduceSumC8u32s  reduceC_<uchar, int,   OpAdd<int> >
2109 #define reduceSumC8u32f  reduceC_<uchar, float, OpAdd<int> >
2110 #define reduceSumC8u64f  reduceC_<uchar, double,OpAdd<int> >
2111 #define reduceSumC16u32f reduceC_<ushort,float, OpAdd<float> >
2112 #define reduceSumC16u64f reduceC_<ushort,double,OpAdd<double> >
2113 #define reduceSumC16s32f reduceC_<short, float, OpAdd<float> >
2114 #define reduceSumC16s64f reduceC_<short, double,OpAdd<double> >
2115 #define reduceSumC32f32f reduceC_<float, float, OpAdd<float> >
2116 #define reduceSumC32f64f reduceC_<float, double,OpAdd<double> >
2117 #define reduceSumC64f64f reduceC_<double,double,OpAdd<double> >
2118
2119 #define reduceMaxC8u  reduceC_<uchar, uchar, OpMax<uchar> >
2120 #define reduceMaxC16u reduceC_<ushort,ushort,OpMax<ushort> >
2121 #define reduceMaxC16s reduceC_<short, short, OpMax<short> >
2122 #define reduceMaxC32f reduceC_<float, float, OpMax<float> >
2123 #define reduceMaxC64f reduceC_<double,double,OpMax<double> >
2124
2125 #define reduceMinC8u  reduceC_<uchar, uchar, OpMin<uchar> >
2126 #define reduceMinC16u reduceC_<ushort,ushort,OpMin<ushort> >
2127 #define reduceMinC16s reduceC_<short, short, OpMin<short> >
2128 #define reduceMinC32f reduceC_<float, float, OpMin<float> >
2129 #define reduceMinC64f reduceC_<double,double,OpMin<double> >
2130
2131 void cv::reduce(InputArray _src, OutputArray _dst, int dim, int op, int dtype)
2132 {
2133     Mat src = _src.getMat();
2134     CV_Assert( src.dims <= 2 );
2135     int op0 = op;
2136     int stype = src.type(), sdepth = src.depth(), cn = src.channels();
2137     if( dtype < 0 )
2138         dtype = _dst.fixedType() ? _dst.type() : stype;
2139     int ddepth = CV_MAT_DEPTH(dtype);
2140
2141     _dst.create(dim == 0 ? 1 : src.rows, dim == 0 ? src.cols : 1,
2142                 CV_MAKETYPE(dtype >= 0 ? dtype : stype, cn));
2143     Mat dst = _dst.getMat(), temp = dst;
2144
2145     CV_Assert( op == CV_REDUCE_SUM || op == CV_REDUCE_MAX ||
2146                op == CV_REDUCE_MIN || op == CV_REDUCE_AVG );
2147     CV_Assert( src.channels() == dst.channels() );
2148
2149     if( op == CV_REDUCE_AVG )
2150     {
2151         op = CV_REDUCE_SUM;
2152         if( sdepth < CV_32S && ddepth < CV_32S )
2153         {
2154             temp.create(dst.rows, dst.cols, CV_32SC(cn));
2155             ddepth = CV_32S;
2156         }
2157     }
2158
2159     ReduceFunc func = 0;
2160     if( dim == 0 )
2161     {
2162         if( op == CV_REDUCE_SUM )
2163         {
2164             if(sdepth == CV_8U && ddepth == CV_32S)
2165                 func = GET_OPTIMIZED(reduceSumR8u32s);
2166             else if(sdepth == CV_8U && ddepth == CV_32F)
2167                 func = reduceSumR8u32f;
2168             else if(sdepth == CV_8U && ddepth == CV_64F)
2169                 func = reduceSumR8u64f;
2170             else if(sdepth == CV_16U && ddepth == CV_32F)
2171                 func = reduceSumR16u32f;
2172             else if(sdepth == CV_16U && ddepth == CV_64F)
2173                 func = reduceSumR16u64f;
2174             else if(sdepth == CV_16S && ddepth == CV_32F)
2175                 func = reduceSumR16s32f;
2176             else if(sdepth == CV_16S && ddepth == CV_64F)
2177                 func = reduceSumR16s64f;
2178             else if(sdepth == CV_32F && ddepth == CV_32F)
2179                 func = GET_OPTIMIZED(reduceSumR32f32f);
2180             else if(sdepth == CV_32F && ddepth == CV_64F)
2181                 func = reduceSumR32f64f;
2182             else if(sdepth == CV_64F && ddepth == CV_64F)
2183                 func = reduceSumR64f64f;
2184         }
2185         else if(op == CV_REDUCE_MAX)
2186         {
2187             if(sdepth == CV_8U && ddepth == CV_8U)
2188                 func = GET_OPTIMIZED(reduceMaxR8u);
2189             else if(sdepth == CV_16U && ddepth == CV_16U)
2190                 func = reduceMaxR16u;
2191             else if(sdepth == CV_16S && ddepth == CV_16S)
2192                 func = reduceMaxR16s;
2193             else if(sdepth == CV_32F && ddepth == CV_32F)
2194                 func = GET_OPTIMIZED(reduceMaxR32f);
2195             else if(sdepth == CV_64F && ddepth == CV_64F)
2196                 func = reduceMaxR64f;
2197         }
2198         else if(op == CV_REDUCE_MIN)
2199         {
2200             if(sdepth == CV_8U && ddepth == CV_8U)
2201                 func = GET_OPTIMIZED(reduceMinR8u);
2202             else if(sdepth == CV_16U && ddepth == CV_16U)
2203                 func = reduceMinR16u;
2204             else if(sdepth == CV_16S && ddepth == CV_16S)
2205                 func = reduceMinR16s;
2206             else if(sdepth == CV_32F && ddepth == CV_32F)
2207                 func = GET_OPTIMIZED(reduceMinR32f);
2208             else if(sdepth == CV_64F && ddepth == CV_64F)
2209                 func = reduceMinR64f;
2210         }
2211     }
2212     else
2213     {
2214         if(op == CV_REDUCE_SUM)
2215         {
2216             if(sdepth == CV_8U && ddepth == CV_32S)
2217                 func = GET_OPTIMIZED(reduceSumC8u32s);
2218             else if(sdepth == CV_8U && ddepth == CV_32F)
2219                 func = reduceSumC8u32f;
2220             else if(sdepth == CV_8U && ddepth == CV_64F)
2221                 func = reduceSumC8u64f;
2222             else if(sdepth == CV_16U && ddepth == CV_32F)
2223                 func = reduceSumC16u32f;
2224             else if(sdepth == CV_16U && ddepth == CV_64F)
2225                 func = reduceSumC16u64f;
2226             else if(sdepth == CV_16S && ddepth == CV_32F)
2227                 func = reduceSumC16s32f;
2228             else if(sdepth == CV_16S && ddepth == CV_64F)
2229                 func = reduceSumC16s64f;
2230             else if(sdepth == CV_32F && ddepth == CV_32F)
2231                 func = GET_OPTIMIZED(reduceSumC32f32f);
2232             else if(sdepth == CV_32F && ddepth == CV_64F)
2233                 func = reduceSumC32f64f;
2234             else if(sdepth == CV_64F && ddepth == CV_64F)
2235                 func = reduceSumC64f64f;
2236         }
2237         else if(op == CV_REDUCE_MAX)
2238         {
2239             if(sdepth == CV_8U && ddepth == CV_8U)
2240                 func = GET_OPTIMIZED(reduceMaxC8u);
2241             else if(sdepth == CV_16U && ddepth == CV_16U)
2242                 func = reduceMaxC16u;
2243             else if(sdepth == CV_16S && ddepth == CV_16S)
2244                 func = reduceMaxC16s;
2245             else if(sdepth == CV_32F && ddepth == CV_32F)
2246                 func = GET_OPTIMIZED(reduceMaxC32f);
2247             else if(sdepth == CV_64F && ddepth == CV_64F)
2248                 func = reduceMaxC64f;
2249         }
2250         else if(op == CV_REDUCE_MIN)
2251         {
2252             if(sdepth == CV_8U && ddepth == CV_8U)
2253                 func = GET_OPTIMIZED(reduceMinC8u);
2254             else if(sdepth == CV_16U && ddepth == CV_16U)
2255                 func = reduceMinC16u;
2256             else if(sdepth == CV_16S && ddepth == CV_16S)
2257                 func = reduceMinC16s;
2258             else if(sdepth == CV_32F && ddepth == CV_32F)
2259                 func = GET_OPTIMIZED(reduceMinC32f);
2260             else if(sdepth == CV_64F && ddepth == CV_64F)
2261                 func = reduceMinC64f;
2262         }
2263     }
2264
2265     if( !func )
2266         CV_Error( CV_StsUnsupportedFormat,
2267                   "Unsupported combination of input and output array formats" );
2268
2269     func( src, temp );
2270
2271     if( op0 == CV_REDUCE_AVG )
2272         temp.convertTo(dst, dst.type(), 1./(dim == 0 ? src.rows : src.cols));
2273 }
2274
2275
2276 //////////////////////////////////////// sort ///////////////////////////////////////////
2277
2278 namespace cv
2279 {
2280
2281 template<typename T> static void sort_( const Mat& src, Mat& dst, int flags )
2282 {
2283     AutoBuffer<T> buf;
2284     T* bptr;
2285     int i, j, n, len;
2286     bool sortRows = (flags & 1) == CV_SORT_EVERY_ROW;
2287     bool inplace = src.data == dst.data;
2288     bool sortDescending = (flags & CV_SORT_DESCENDING) != 0;
2289
2290     if( sortRows )
2291         n = src.rows, len = src.cols;
2292     else
2293     {
2294         n = src.cols, len = src.rows;
2295         buf.allocate(len);
2296     }
2297     bptr = (T*)buf;
2298
2299     for( i = 0; i < n; i++ )
2300     {
2301         T* ptr = bptr;
2302         if( sortRows )
2303         {
2304             T* dptr = (T*)(dst.data + dst.step*i);
2305             if( !inplace )
2306             {
2307                 const T* sptr = (const T*)(src.data + src.step*i);
2308                 for( j = 0; j < len; j++ )
2309                     dptr[j] = sptr[j];
2310             }
2311             ptr = dptr;
2312         }
2313         else
2314         {
2315             for( j = 0; j < len; j++ )
2316                 ptr[j] = ((const T*)(src.data + src.step*j))[i];
2317         }
2318         std::sort( ptr, ptr + len, LessThan<T>() );
2319         if( sortDescending )
2320             for( j = 0; j < len/2; j++ )
2321                 std::swap(ptr[j], ptr[len-1-j]);
2322         if( !sortRows )
2323             for( j = 0; j < len; j++ )
2324                 ((T*)(dst.data + dst.step*j))[i] = ptr[j];
2325     }
2326 }
2327
2328
2329 template<typename T> static void sortIdx_( const Mat& src, Mat& dst, int flags )
2330 {
2331     AutoBuffer<T> buf;
2332     AutoBuffer<int> ibuf;
2333     T* bptr;
2334     int* _iptr;
2335     int i, j, n, len;
2336     bool sortRows = (flags & 1) == CV_SORT_EVERY_ROW;
2337     bool sortDescending = (flags & CV_SORT_DESCENDING) != 0;
2338
2339     CV_Assert( src.data != dst.data );
2340
2341     if( sortRows )
2342         n = src.rows, len = src.cols;
2343     else
2344     {
2345         n = src.cols, len = src.rows;
2346         buf.allocate(len);
2347         ibuf.allocate(len);
2348     }
2349     bptr = (T*)buf;
2350     _iptr = (int*)ibuf;
2351
2352     for( i = 0; i < n; i++ )
2353     {
2354         T* ptr = bptr;
2355         int* iptr = _iptr;
2356
2357         if( sortRows )
2358         {
2359             ptr = (T*)(src.data + src.step*i);
2360             iptr = (int*)(dst.data + dst.step*i);
2361         }
2362         else
2363         {
2364             for( j = 0; j < len; j++ )
2365                 ptr[j] = ((const T*)(src.data + src.step*j))[i];
2366         }
2367         for( j = 0; j < len; j++ )
2368             iptr[j] = j;
2369         std::sort( iptr, iptr + len, LessThanIdx<T>(ptr) );
2370         if( sortDescending )
2371             for( j = 0; j < len/2; j++ )
2372                 std::swap(iptr[j], iptr[len-1-j]);
2373         if( !sortRows )
2374             for( j = 0; j < len; j++ )
2375                 ((int*)(dst.data + dst.step*j))[i] = iptr[j];
2376     }
2377 }
2378
2379 typedef void (*SortFunc)(const Mat& src, Mat& dst, int flags);
2380
2381 }
2382
2383 void cv::sort( InputArray _src, OutputArray _dst, int flags )
2384 {
2385     static SortFunc tab[] =
2386     {
2387         sort_<uchar>, sort_<schar>, sort_<ushort>, sort_<short>,
2388         sort_<int>, sort_<float>, sort_<double>, 0
2389     };
2390     Mat src = _src.getMat();
2391     SortFunc func = tab[src.depth()];
2392     CV_Assert( src.dims <= 2 && src.channels() == 1 && func != 0 );
2393     _dst.create( src.size(), src.type() );
2394     Mat dst = _dst.getMat();
2395     func( src, dst, flags );
2396 }
2397
2398 void cv::sortIdx( InputArray _src, OutputArray _dst, int flags )
2399 {
2400     static SortFunc tab[] =
2401     {
2402         sortIdx_<uchar>, sortIdx_<schar>, sortIdx_<ushort>, sortIdx_<short>,
2403         sortIdx_<int>, sortIdx_<float>, sortIdx_<double>, 0
2404     };
2405     Mat src = _src.getMat();
2406     SortFunc func = tab[src.depth()];
2407     CV_Assert( src.dims <= 2 && src.channels() == 1 && func != 0 );
2408
2409     Mat dst = _dst.getMat();
2410     if( dst.data == src.data )
2411         _dst.release();
2412     _dst.create( src.size(), CV_32S );
2413     dst = _dst.getMat();
2414     func( src, dst, flags );
2415 }
2416
2417
2418 ////////////////////////////////////////// kmeans ////////////////////////////////////////////
2419
2420 namespace cv
2421 {
2422
2423 static void generateRandomCenter(const vector<Vec2f>& box, float* center, RNG& rng)
2424 {
2425     size_t j, dims = box.size();
2426     float margin = 1.f/dims;
2427     for( j = 0; j < dims; j++ )
2428         center[j] = ((float)rng*(1.f+margin*2.f)-margin)*(box[j][1] - box[j][0]) + box[j][0];
2429 }
2430
2431 class KMeansPPDistanceComputer
2432 {
2433 public:
2434     KMeansPPDistanceComputer( float *_tdist2,
2435                               const float *_data,
2436                               const float *_dist,
2437                               int _dims,
2438                               size_t _step,
2439                               size_t _stepci )
2440         : tdist2(_tdist2),
2441           data(_data),
2442           dist(_dist),
2443           dims(_dims),
2444           step(_step),
2445           stepci(_stepci) { }
2446
2447     void operator()( const cv::BlockedRange& range ) const
2448     {
2449         const int begin = range.begin();
2450         const int end = range.end();
2451
2452         for ( int i = begin; i<end; i++ )
2453         {
2454             tdist2[i] = std::min(normL2Sqr_(data + step*i, data + stepci, dims), dist[i]);
2455         }
2456     }
2457
2458 private:
2459     KMeansPPDistanceComputer& operator=(const KMeansPPDistanceComputer&); // to quiet MSVC
2460
2461     float *tdist2;
2462     const float *data;
2463     const float *dist;
2464     const int dims;
2465     const size_t step;
2466     const size_t stepci;
2467 };
2468
2469 /*
2470 k-means center initialization using the following algorithm:
2471 Arthur & Vassilvitskii (2007) k-means++: The Advantages of Careful Seeding
2472 */
2473 static void generateCentersPP(const Mat& _data, Mat& _out_centers,
2474                               int K, RNG& rng, int trials)
2475 {
2476     int i, j, k, dims = _data.cols, N = _data.rows;
2477     const float* data = _data.ptr<float>(0);
2478     size_t step = _data.step/sizeof(data[0]);
2479     vector<int> _centers(K);
2480     int* centers = &_centers[0];
2481     vector<float> _dist(N*3);
2482     float* dist = &_dist[0], *tdist = dist + N, *tdist2 = tdist + N;
2483     double sum0 = 0;
2484
2485     centers[0] = (unsigned)rng % N;
2486
2487     for( i = 0; i < N; i++ )
2488     {
2489         dist[i] = normL2Sqr_(data + step*i, data + step*centers[0], dims);
2490         sum0 += dist[i];
2491     }
2492
2493     for( k = 1; k < K; k++ )
2494     {
2495         double bestSum = DBL_MAX;
2496         int bestCenter = -1;
2497
2498         for( j = 0; j < trials; j++ )
2499         {
2500             double p = (double)rng*sum0, s = 0;
2501             for( i = 0; i < N-1; i++ )
2502                 if( (p -= dist[i]) <= 0 )
2503                     break;
2504             int ci = i;
2505
2506             parallel_for(BlockedRange(0, N),
2507                          KMeansPPDistanceComputer(tdist2, data, dist, dims, step, step*ci));
2508             for( i = 0; i < N; i++ )
2509             {
2510                 s += tdist2[i];
2511             }
2512
2513             if( s < bestSum )
2514             {
2515                 bestSum = s;
2516                 bestCenter = ci;
2517                 std::swap(tdist, tdist2);
2518             }
2519         }
2520         centers[k] = bestCenter;
2521         sum0 = bestSum;
2522         std::swap(dist, tdist);
2523     }
2524
2525     for( k = 0; k < K; k++ )
2526     {
2527         const float* src = data + step*centers[k];
2528         float* dst = _out_centers.ptr<float>(k);
2529         for( j = 0; j < dims; j++ )
2530             dst[j] = src[j];
2531     }
2532 }
2533
2534 class KMeansDistanceComputer
2535 {
2536 public:
2537     KMeansDistanceComputer( double *_distances,
2538                             int *_labels,
2539                             const Mat& _data,
2540                             const Mat& _centers )
2541         : distances(_distances),
2542           labels(_labels),
2543           data(_data),
2544           centers(_centers)
2545     {
2546         CV_DbgAssert(centers.cols == data.cols);
2547     }
2548
2549     void operator()( const BlockedRange& range ) const
2550     {
2551         const int begin = range.begin();
2552         const int end = range.end();
2553         const int K = centers.rows;
2554         const int dims = centers.cols;
2555
2556         const float *sample;
2557         for( int i = begin; i<end; ++i)
2558         {
2559             sample = data.ptr<float>(i);
2560             int k_best = 0;
2561             double min_dist = DBL_MAX;
2562
2563             for( int k = 0; k < K; k++ )
2564             {
2565                 const float* center = centers.ptr<float>(k);
2566                 const double dist = normL2Sqr_(sample, center, dims);
2567
2568                 if( min_dist > dist )
2569                 {
2570                     min_dist = dist;
2571                     k_best = k;
2572                 }
2573             }
2574
2575             distances[i] = min_dist;
2576             labels[i] = k_best;
2577         }
2578     }
2579
2580 private:
2581     KMeansDistanceComputer& operator=(const KMeansDistanceComputer&); // to quiet MSVC
2582
2583     double *distances;
2584     int *labels;
2585     const Mat& data;
2586     const Mat& centers;
2587 };
2588
2589 }
2590
2591 double cv::kmeans( InputArray _data, int K,
2592                    InputOutputArray _bestLabels,
2593                    TermCriteria criteria, int attempts,
2594                    int flags, OutputArray _centers )
2595 {
2596     const int SPP_TRIALS = 3;
2597     Mat data = _data.getMat();
2598     bool isrow = data.rows == 1 && data.channels() > 1;
2599     int N = !isrow ? data.rows : data.cols;
2600     int dims = (!isrow ? data.cols : 1)*data.channels();
2601     int type = data.depth();
2602
2603     attempts = std::max(attempts, 1);
2604     CV_Assert( data.dims <= 2 && type == CV_32F && K > 0 );
2605     CV_Assert( N >= K );
2606
2607     _bestLabels.create(N, 1, CV_32S, -1, true);
2608
2609     Mat _labels, best_labels = _bestLabels.getMat();
2610     if( flags & CV_KMEANS_USE_INITIAL_LABELS )
2611     {
2612         CV_Assert( (best_labels.cols == 1 || best_labels.rows == 1) &&
2613                   best_labels.cols*best_labels.rows == N &&
2614                   best_labels.type() == CV_32S &&
2615                   best_labels.isContinuous());
2616         best_labels.copyTo(_labels);
2617     }
2618     else
2619     {
2620         if( !((best_labels.cols == 1 || best_labels.rows == 1) &&
2621              best_labels.cols*best_labels.rows == N &&
2622             best_labels.type() == CV_32S &&
2623             best_labels.isContinuous()))
2624             best_labels.create(N, 1, CV_32S);
2625         _labels.create(best_labels.size(), best_labels.type());
2626     }
2627     int* labels = _labels.ptr<int>();
2628
2629     Mat centers(K, dims, type), old_centers(K, dims, type), temp(1, dims, type);
2630     vector<int> counters(K);
2631     vector<Vec2f> _box(dims);
2632     Vec2f* box = &_box[0];
2633     double best_compactness = DBL_MAX, compactness = 0;
2634     RNG& rng = theRNG();
2635     int a, iter, i, j, k;
2636
2637     if( criteria.type & TermCriteria::EPS )
2638         criteria.epsilon = std::max(criteria.epsilon, 0.);
2639     else
2640         criteria.epsilon = FLT_EPSILON;
2641     criteria.epsilon *= criteria.epsilon;
2642
2643     if( criteria.type & TermCriteria::COUNT )
2644         criteria.maxCount = std::min(std::max(criteria.maxCount, 2), 100);
2645     else
2646         criteria.maxCount = 100;
2647
2648     if( K == 1 )
2649     {
2650         attempts = 1;
2651         criteria.maxCount = 2;
2652     }
2653
2654     const float* sample = data.ptr<float>(0);
2655     for( j = 0; j < dims; j++ )
2656         box[j] = Vec2f(sample[j], sample[j]);
2657
2658     for( i = 1; i < N; i++ )
2659     {
2660         sample = data.ptr<float>(i);
2661         for( j = 0; j < dims; j++ )
2662         {
2663             float v = sample[j];
2664             box[j][0] = std::min(box[j][0], v);
2665             box[j][1] = std::max(box[j][1], v);
2666         }
2667     }
2668
2669     for( a = 0; a < attempts; a++ )
2670     {
2671         double max_center_shift = DBL_MAX;
2672         for( iter = 0;; )
2673         {
2674             swap(centers, old_centers);
2675
2676             if( iter == 0 && (a > 0 || !(flags & KMEANS_USE_INITIAL_LABELS)) )
2677             {
2678                 if( flags & KMEANS_PP_CENTERS )
2679                     generateCentersPP(data, centers, K, rng, SPP_TRIALS);
2680                 else
2681                 {
2682                     for( k = 0; k < K; k++ )
2683                         generateRandomCenter(_box, centers.ptr<float>(k), rng);
2684                 }
2685             }
2686             else
2687             {
2688                 if( iter == 0 && a == 0 && (flags & KMEANS_USE_INITIAL_LABELS) )
2689                 {
2690                     for( i = 0; i < N; i++ )
2691                         CV_Assert( (unsigned)labels[i] < (unsigned)K );
2692                 }
2693
2694                 // compute centers
2695                 centers = Scalar(0);
2696                 for( k = 0; k < K; k++ )
2697                     counters[k] = 0;
2698
2699                 for( i = 0; i < N; i++ )
2700                 {
2701                     sample = data.ptr<float>(i);
2702                     k = labels[i];
2703                     float* center = centers.ptr<float>(k);
2704                     j=0;
2705                     #if CV_ENABLE_UNROLLED
2706                     for(; j <= dims - 4; j += 4 )
2707                     {
2708                         float t0 = center[j] + sample[j];
2709                         float t1 = center[j+1] + sample[j+1];
2710
2711                         center[j] = t0;
2712                         center[j+1] = t1;
2713
2714                         t0 = center[j+2] + sample[j+2];
2715                         t1 = center[j+3] + sample[j+3];
2716
2717                         center[j+2] = t0;
2718                         center[j+3] = t1;
2719                     }
2720                     #endif
2721                     for( ; j < dims; j++ )
2722                         center[j] += sample[j];
2723                     counters[k]++;
2724                 }
2725
2726                 if( iter > 0 )
2727                     max_center_shift = 0;
2728
2729                 for( k = 0; k < K; k++ )
2730                 {
2731                     if( counters[k] != 0 )
2732                         continue;
2733
2734                     // if some cluster appeared to be empty then:
2735                     //   1. find the biggest cluster
2736                     //   2. find the farthest from the center point in the biggest cluster
2737                     //   3. exclude the farthest point from the biggest cluster and form a new 1-point cluster.
2738                     int max_k = 0;
2739                     for( int k1 = 1; k1 < K; k1++ )
2740                     {
2741                         if( counters[max_k] < counters[k1] )
2742                             max_k = k1;
2743                     }
2744
2745                     double max_dist = 0;
2746                     int farthest_i = -1;
2747                     float* new_center = centers.ptr<float>(k);
2748                     float* old_center = centers.ptr<float>(max_k);
2749                     float* _old_center = temp.ptr<float>(); // normalized
2750                     float scale = 1.f/counters[max_k];
2751                     for( j = 0; j < dims; j++ )
2752                         _old_center[j] = old_center[j]*scale;
2753
2754                     for( i = 0; i < N; i++ )
2755                     {
2756                         if( labels[i] != max_k )
2757                             continue;
2758                         sample = data.ptr<float>(i);
2759                         double dist = normL2Sqr_(sample, _old_center, dims);
2760
2761                         if( max_dist <= dist )
2762                         {
2763                             max_dist = dist;
2764                             farthest_i = i;
2765                         }
2766                     }
2767
2768                     counters[max_k]--;
2769                     counters[k]++;
2770                     labels[farthest_i] = k;
2771                     sample = data.ptr<float>(farthest_i);
2772
2773                     for( j = 0; j < dims; j++ )
2774                     {
2775                         old_center[j] -= sample[j];
2776                         new_center[j] += sample[j];
2777                     }
2778                 }
2779
2780                 for( k = 0; k < K; k++ )
2781                 {
2782                     float* center = centers.ptr<float>(k);
2783                     CV_Assert( counters[k] != 0 );
2784
2785                     float scale = 1.f/counters[k];
2786                     for( j = 0; j < dims; j++ )
2787                         center[j] *= scale;
2788
2789                     if( iter > 0 )
2790                     {
2791                         double dist = 0;
2792                         const float* old_center = old_centers.ptr<float>(k);
2793                         for( j = 0; j < dims; j++ )
2794                         {
2795                             double t = center[j] - old_center[j];
2796                             dist += t*t;
2797                         }
2798                         max_center_shift = std::max(max_center_shift, dist);
2799                     }
2800                 }
2801             }
2802
2803             if( ++iter == MAX(criteria.maxCount, 2) || max_center_shift <= criteria.epsilon )
2804                 break;
2805
2806             // assign labels
2807             Mat dists(1, N, CV_64F);
2808             double* dist = dists.ptr<double>(0);
2809             parallel_for(BlockedRange(0, N),
2810                          KMeansDistanceComputer(dist, labels, data, centers));
2811             compactness = 0;
2812             for( i = 0; i < N; i++ )
2813             {
2814                 compactness += dist[i];
2815             }
2816         }
2817
2818         if( compactness < best_compactness )
2819         {
2820             best_compactness = compactness;
2821             if( _centers.needed() )
2822                 centers.copyTo(_centers);
2823             _labels.copyTo(best_labels);
2824         }
2825     }
2826
2827     return best_compactness;
2828 }
2829
2830
2831 CV_IMPL void cvSetIdentity( CvArr* arr, CvScalar value )
2832 {
2833     cv::Mat m = cv::cvarrToMat(arr);
2834     cv::setIdentity(m, value);
2835 }
2836
2837
2838 CV_IMPL CvScalar cvTrace( const CvArr* arr )
2839 {
2840     return cv::trace(cv::cvarrToMat(arr));
2841 }
2842
2843
2844 CV_IMPL void cvTranspose( const CvArr* srcarr, CvArr* dstarr )
2845 {
2846     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
2847
2848     CV_Assert( src.rows == dst.cols && src.cols == dst.rows && src.type() == dst.type() );
2849     transpose( src, dst );
2850 }
2851
2852
2853 CV_IMPL void cvCompleteSymm( CvMat* matrix, int LtoR )
2854 {
2855     cv::Mat m(matrix);
2856     cv::completeSymm( m, LtoR != 0 );
2857 }
2858
2859
2860 CV_IMPL void cvCrossProduct( const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr )
2861 {
2862     cv::Mat srcA = cv::cvarrToMat(srcAarr), dst = cv::cvarrToMat(dstarr);
2863
2864     CV_Assert( srcA.size() == dst.size() && srcA.type() == dst.type() );
2865     srcA.cross(cv::cvarrToMat(srcBarr)).copyTo(dst);
2866 }
2867
2868
2869 CV_IMPL void
2870 cvReduce( const CvArr* srcarr, CvArr* dstarr, int dim, int op )
2871 {
2872     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
2873
2874     if( dim < 0 )
2875         dim = src.rows > dst.rows ? 0 : src.cols > dst.cols ? 1 : dst.cols == 1;
2876
2877     if( dim > 1 )
2878         CV_Error( CV_StsOutOfRange, "The reduced dimensionality index is out of range" );
2879
2880     if( (dim == 0 && (dst.cols != src.cols || dst.rows != 1)) ||
2881         (dim == 1 && (dst.rows != src.rows || dst.cols != 1)) )
2882         CV_Error( CV_StsBadSize, "The output array size is incorrect" );
2883
2884     if( src.channels() != dst.channels() )
2885         CV_Error( CV_StsUnmatchedFormats, "Input and output arrays must have the same number of channels" );
2886
2887     cv::reduce(src, dst, dim, op, dst.type());
2888 }
2889
2890
2891 CV_IMPL CvArr*
2892 cvRange( CvArr* arr, double start, double end )
2893 {
2894     int ok = 0;
2895
2896     CvMat stub, *mat = (CvMat*)arr;
2897     double delta;
2898     int type, step;
2899     double val = start;
2900     int i, j;
2901     int rows, cols;
2902
2903     if( !CV_IS_MAT(mat) )
2904         mat = cvGetMat( mat, &stub);
2905
2906     rows = mat->rows;
2907     cols = mat->cols;
2908     type = CV_MAT_TYPE(mat->type);
2909     delta = (end-start)/(rows*cols);
2910
2911     if( CV_IS_MAT_CONT(mat->type) )
2912     {
2913         cols *= rows;
2914         rows = 1;
2915         step = 1;
2916     }
2917     else
2918         step = mat->step / CV_ELEM_SIZE(type);
2919
2920     if( type == CV_32SC1 )
2921     {
2922         int* idata = mat->data.i;
2923         int ival = cvRound(val), idelta = cvRound(delta);
2924
2925         if( fabs(val - ival) < DBL_EPSILON &&
2926             fabs(delta - idelta) < DBL_EPSILON )
2927         {
2928             for( i = 0; i < rows; i++, idata += step )
2929                 for( j = 0; j < cols; j++, ival += idelta )
2930                     idata[j] = ival;
2931         }
2932         else
2933         {
2934             for( i = 0; i < rows; i++, idata += step )
2935                 for( j = 0; j < cols; j++, val += delta )
2936                     idata[j] = cvRound(val);
2937         }
2938     }
2939     else if( type == CV_32FC1 )
2940     {
2941         float* fdata = mat->data.fl;
2942         for( i = 0; i < rows; i++, fdata += step )
2943             for( j = 0; j < cols; j++, val += delta )
2944                 fdata[j] = (float)val;
2945     }
2946     else
2947         CV_Error( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" );
2948
2949     ok = 1;
2950     return ok ? arr : 0;
2951 }
2952
2953
2954 CV_IMPL void
2955 cvSort( const CvArr* _src, CvArr* _dst, CvArr* _idx, int flags )
2956 {
2957     cv::Mat src = cv::cvarrToMat(_src);
2958
2959     if( _idx )
2960     {
2961         cv::Mat idx0 = cv::cvarrToMat(_idx), idx = idx0;
2962         CV_Assert( src.size() == idx.size() && idx.type() == CV_32S && src.data != idx.data );
2963         cv::sortIdx( src, idx, flags );
2964         CV_Assert( idx0.data == idx.data );
2965     }
2966
2967     if( _dst )
2968     {
2969         cv::Mat dst0 = cv::cvarrToMat(_dst), dst = dst0;
2970         CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
2971         cv::sort( src, dst, flags );
2972         CV_Assert( dst0.data == dst.data );
2973     }
2974 }
2975
2976
2977 CV_IMPL int
2978 cvKMeans2( const CvArr* _samples, int cluster_count, CvArr* _labels,
2979            CvTermCriteria termcrit, int attempts, CvRNG*,
2980            int flags, CvArr* _centers, double* _compactness )
2981 {
2982     cv::Mat data = cv::cvarrToMat(_samples), labels = cv::cvarrToMat(_labels), centers;
2983     if( _centers )
2984     {
2985         centers = cv::cvarrToMat(_centers);
2986
2987         centers = centers.reshape(1);
2988         data = data.reshape(1);
2989
2990         CV_Assert( !centers.empty() );
2991         CV_Assert( centers.rows == cluster_count );
2992         CV_Assert( centers.cols == data.cols );
2993         CV_Assert( centers.depth() == data.depth() );
2994     }
2995     CV_Assert( labels.isContinuous() && labels.type() == CV_32S &&
2996         (labels.cols == 1 || labels.rows == 1) &&
2997         labels.cols + labels.rows - 1 == data.rows );
2998
2999     double compactness = cv::kmeans(data, cluster_count, labels, termcrit, attempts,
3000                                     flags, _centers ? cv::_OutputArray(centers) : cv::_OutputArray() );
3001     if( _compactness )
3002         *_compactness = compactness;
3003     return 1;
3004 }
3005
3006 ///////////////////////////// n-dimensional matrices ////////////////////////////
3007
3008 namespace cv
3009 {
3010
3011 Mat Mat::reshape(int _cn, int _newndims, const int* _newsz) const
3012 {
3013     if(_newndims == dims)
3014     {
3015         if(_newsz == 0)
3016             return reshape(_cn);
3017         if(_newndims == 2)
3018             return reshape(_cn, _newsz[0]);
3019     }
3020
3021     CV_Error(CV_StsNotImplemented, "");
3022     // TBD
3023     return Mat();
3024 }
3025
3026 Mat::operator CvMatND() const
3027 {
3028     CvMatND mat;
3029     cvInitMatNDHeader( &mat, dims, size, type(), data );
3030     int i, d = dims;
3031     for( i = 0; i < d; i++ )
3032         mat.dim[i].step = (int)step[i];
3033     mat.type |= flags & CONTINUOUS_FLAG;
3034     return mat;
3035 }
3036
3037 NAryMatIterator::NAryMatIterator()
3038     : arrays(0), planes(0), ptrs(0), narrays(0), nplanes(0), size(0), iterdepth(0), idx(0)
3039 {
3040 }
3041
3042 NAryMatIterator::NAryMatIterator(const Mat** _arrays, Mat* _planes, int _narrays)
3043 : arrays(0), planes(0), ptrs(0), narrays(0), nplanes(0), size(0), iterdepth(0), idx(0)
3044 {
3045     init(_arrays, _planes, 0, _narrays);
3046 }
3047
3048 NAryMatIterator::NAryMatIterator(const Mat** _arrays, uchar** _ptrs, int _narrays)
3049     : arrays(0), planes(0), ptrs(0), narrays(0), nplanes(0), size(0), iterdepth(0), idx(0)
3050 {
3051     init(_arrays, 0, _ptrs, _narrays);
3052 }
3053
3054 void NAryMatIterator::init(const Mat** _arrays, Mat* _planes, uchar** _ptrs, int _narrays)
3055 {
3056     CV_Assert( _arrays && (_ptrs || _planes) );
3057     int i, j, d1=0, i0 = -1, d = -1;
3058
3059     arrays = _arrays;
3060     ptrs = _ptrs;
3061     planes = _planes;
3062     narrays = _narrays;
3063     nplanes = 0;
3064     size = 0;
3065
3066     if( narrays < 0 )
3067     {
3068         for( i = 0; _arrays[i] != 0; i++ )
3069             ;
3070         narrays = i;
3071         CV_Assert(narrays <= 1000);
3072     }
3073
3074     iterdepth = 0;
3075
3076     for( i = 0; i < narrays; i++ )
3077     {
3078         CV_Assert(arrays[i] != 0);
3079         const Mat& A = *arrays[i];
3080         if( ptrs )
3081             ptrs[i] = A.data;
3082
3083         if( !A.data )
3084             continue;
3085
3086         if( i0 < 0 )
3087         {
3088             i0 = i;
3089             d = A.dims;
3090
3091             // find the first dimensionality which is different from 1;
3092             // in any of the arrays the first "d1" step do not affect the continuity
3093             for( d1 = 0; d1 < d; d1++ )
3094                 if( A.size[d1] > 1 )
3095                     break;
3096         }
3097         else
3098             CV_Assert( A.size == arrays[i0]->size );
3099
3100         if( !A.isContinuous() )
3101         {
3102             CV_Assert( A.step[d-1] == A.elemSize() );
3103             for( j = d-1; j > d1; j-- )
3104                 if( A.step[j]*A.size[j] < A.step[j-1] )
3105                     break;
3106             iterdepth = std::max(iterdepth, j);
3107         }
3108     }
3109
3110     if( i0 >= 0 )
3111     {
3112         size = arrays[i0]->size[d-1];
3113         for( j = d-1; j > iterdepth; j-- )
3114         {
3115             int64 total1 = (int64)size*arrays[i0]->size[j-1];
3116             if( total1 != (int)total1 )
3117                 break;
3118             size = (int)total1;
3119         }
3120
3121         iterdepth = j;
3122         if( iterdepth == d1 )
3123             iterdepth = 0;
3124
3125         nplanes = 1;
3126         for( j = iterdepth-1; j >= 0; j-- )
3127             nplanes *= arrays[i0]->size[j];
3128     }
3129     else
3130         iterdepth = 0;
3131
3132     idx = 0;
3133
3134     if( !planes )
3135         return;
3136
3137     for( i = 0; i < narrays; i++ )
3138     {
3139         CV_Assert(arrays[i] != 0);
3140         const Mat& A = *arrays[i];
3141
3142         if( !A.data )
3143         {
3144             planes[i] = Mat();
3145             continue;
3146         }
3147
3148         planes[i] = Mat(1, (int)size, A.type(), A.data);
3149     }
3150 }
3151
3152
3153 NAryMatIterator& NAryMatIterator::operator ++()
3154 {
3155     if( idx >= nplanes-1 )
3156         return *this;
3157     ++idx;
3158
3159     if( iterdepth == 1 )
3160     {
3161         if( ptrs )
3162         {
3163             for( int i = 0; i < narrays; i++ )
3164             {
3165                 if( !ptrs[i] )
3166                     continue;
3167                 ptrs[i] = arrays[i]->data + arrays[i]->step[0]*idx;
3168             }
3169         }
3170         if( planes )
3171         {
3172             for( int i = 0; i < narrays; i++ )
3173             {
3174                 if( !planes[i].data )
3175                     continue;
3176                 planes[i].data = arrays[i]->data + arrays[i]->step[0]*idx;
3177             }
3178         }
3179     }
3180     else
3181     {
3182         for( int i = 0; i < narrays; i++ )
3183         {
3184             const Mat& A = *arrays[i];
3185             if( !A.data )
3186                 continue;
3187             int _idx = (int)idx;
3188             uchar* data = A.data;
3189             for( int j = iterdepth-1; j >= 0 && _idx > 0; j-- )
3190             {
3191                 int szi = A.size[j], t = _idx/szi;
3192                 data += (_idx - t * szi)*A.step[j];
3193                 _idx = t;
3194             }
3195             if( ptrs )
3196                 ptrs[i] = data;
3197             if( planes )
3198                 planes[i].data = data;
3199         }
3200     }
3201
3202     return *this;
3203 }
3204
3205 NAryMatIterator NAryMatIterator::operator ++(int)
3206 {
3207     NAryMatIterator it = *this;
3208     ++*this;
3209     return it;
3210 }
3211
3212 ///////////////////////////////////////////////////////////////////////////
3213 //                              MatConstIterator                         //
3214 ///////////////////////////////////////////////////////////////////////////
3215
3216 Point MatConstIterator::pos() const
3217 {
3218     if( !m )
3219         return Point();
3220     CV_DbgAssert(m->dims <= 2);
3221
3222     ptrdiff_t ofs = ptr - m->data;
3223     int y = (int)(ofs/m->step[0]);
3224     return Point((int)((ofs - y*m->step[0])/elemSize), y);
3225 }
3226
3227 void MatConstIterator::pos(int* _idx) const
3228 {
3229     CV_Assert(m != 0 && _idx);
3230     ptrdiff_t ofs = ptr - m->data;
3231     for( int i = 0; i < m->dims; i++ )
3232     {
3233         size_t s = m->step[i], v = ofs/s;
3234         ofs -= v*s;
3235         _idx[i] = (int)v;
3236     }
3237 }
3238
3239 ptrdiff_t MatConstIterator::lpos() const
3240 {
3241     if(!m)
3242         return 0;
3243     if( m->isContinuous() )
3244         return (ptr - sliceStart)/elemSize;
3245     ptrdiff_t ofs = ptr - m->data;
3246     int i, d = m->dims;
3247     if( d == 2 )
3248     {
3249         ptrdiff_t y = ofs/m->step[0];
3250         return y*m->cols + (ofs - y*m->step[0])/elemSize;
3251     }
3252     ptrdiff_t result = 0;
3253     for( i = 0; i < d; i++ )
3254     {
3255         size_t s = m->step[i], v = ofs/s;
3256         ofs -= v*s;
3257         result = result*m->size[i] + v;
3258     }
3259     return result;
3260 }
3261
3262 void MatConstIterator::seek(ptrdiff_t ofs, bool relative)
3263 {
3264     if( m->isContinuous() )
3265     {
3266         ptr = (relative ? ptr : sliceStart) + ofs*elemSize;
3267         if( ptr < sliceStart )
3268             ptr = sliceStart;
3269         else if( ptr > sliceEnd )
3270             ptr = sliceEnd;
3271         return;
3272     }
3273
3274     int d = m->dims;
3275     if( d == 2 )
3276     {
3277         ptrdiff_t ofs0, y;
3278         if( relative )
3279         {
3280             ofs0 = ptr - m->data;
3281             y = ofs0/m->step[0];
3282             ofs += y*m->cols + (ofs0 - y*m->step[0])/elemSize;
3283         }
3284         y = ofs/m->cols;
3285         int y1 = std::min(std::max((int)y, 0), m->rows-1);
3286         sliceStart = m->data + y1*m->step[0];
3287         sliceEnd = sliceStart + m->cols*elemSize;
3288         ptr = y < 0 ? sliceStart : y >= m->rows ? sliceEnd :
3289             sliceStart + (ofs - y*m->cols)*elemSize;
3290         return;
3291     }
3292
3293     if( relative )
3294         ofs += lpos();
3295
3296     if( ofs < 0 )
3297         ofs = 0;
3298
3299     int szi = m->size[d-1];
3300     ptrdiff_t t = ofs/szi;
3301     int v = (int)(ofs - t*szi);
3302     ofs = t;
3303     ptr = m->data + v*elemSize;
3304     sliceStart = m->data;
3305
3306     for( int i = d-2; i >= 0; i-- )
3307     {
3308         szi = m->size[i];
3309         t = ofs/szi;
3310         v = (int)(ofs - t*szi);
3311         ofs = t;
3312         sliceStart += v*m->step[i];
3313     }
3314
3315     sliceEnd = sliceStart + m->size[d-1]*elemSize;
3316     if( ofs > 0 )
3317         ptr = sliceEnd;
3318     else
3319         ptr = sliceStart + (ptr - m->data);
3320 }
3321
3322 void MatConstIterator::seek(const int* _idx, bool relative)
3323 {
3324     int i, d = m->dims;
3325     ptrdiff_t ofs = 0;
3326     if( !_idx )
3327         ;
3328     else if( d == 2 )
3329         ofs = _idx[0]*m->size[1] + _idx[1];
3330     else
3331     {
3332         for( i = 0; i < d; i++ )
3333             ofs = ofs*m->size[i] + _idx[i];
3334     }
3335     seek(ofs, relative);
3336 }
3337
3338 ptrdiff_t operator - (const MatConstIterator& b, const MatConstIterator& a)
3339 {
3340     if( a.m != b.m )
3341         return INT_MAX;
3342     if( a.sliceEnd == b.sliceEnd )
3343         return (b.ptr - a.ptr)/b.elemSize;
3344
3345     return b.lpos() - a.lpos();
3346 }
3347
3348 //////////////////////////////// SparseMat ////////////////////////////////
3349
3350 template<typename T1, typename T2> void
3351 convertData_(const void* _from, void* _to, int cn)
3352 {
3353     const T1* from = (const T1*)_from;
3354     T2* to = (T2*)_to;
3355     if( cn == 1 )
3356         *to = saturate_cast<T2>(*from);
3357     else
3358         for( int i = 0; i < cn; i++ )
3359             to[i] = saturate_cast<T2>(from[i]);
3360 }
3361
3362 template<typename T1, typename T2> void
3363 convertScaleData_(const void* _from, void* _to, int cn, double alpha, double beta)
3364 {
3365     const T1* from = (const T1*)_from;
3366     T2* to = (T2*)_to;
3367     if( cn == 1 )
3368         *to = saturate_cast<T2>(*from*alpha + beta);
3369     else
3370         for( int i = 0; i < cn; i++ )
3371             to[i] = saturate_cast<T2>(from[i]*alpha + beta);
3372 }
3373
3374 ConvertData getConvertElem(int fromType, int toType)
3375 {
3376     static ConvertData tab[][8] =
3377     {{ convertData_<uchar, uchar>, convertData_<uchar, schar>,
3378       convertData_<uchar, ushort>, convertData_<uchar, short>,
3379       convertData_<uchar, int>, convertData_<uchar, float>,
3380       convertData_<uchar, double>, 0 },
3381
3382     { convertData_<schar, uchar>, convertData_<schar, schar>,
3383       convertData_<schar, ushort>, convertData_<schar, short>,
3384       convertData_<schar, int>, convertData_<schar, float>,
3385       convertData_<schar, double>, 0 },
3386
3387     { convertData_<ushort, uchar>, convertData_<ushort, schar>,
3388       convertData_<ushort, ushort>, convertData_<ushort, short>,
3389       convertData_<ushort, int>, convertData_<ushort, float>,
3390       convertData_<ushort, double>, 0 },
3391
3392     { convertData_<short, uchar>, convertData_<short, schar>,
3393       convertData_<short, ushort>, convertData_<short, short>,
3394       convertData_<short, int>, convertData_<short, float>,
3395       convertData_<short, double>, 0 },
3396
3397     { convertData_<int, uchar>, convertData_<int, schar>,
3398       convertData_<int, ushort>, convertData_<int, short>,
3399       convertData_<int, int>, convertData_<int, float>,
3400       convertData_<int, double>, 0 },
3401
3402     { convertData_<float, uchar>, convertData_<float, schar>,
3403       convertData_<float, ushort>, convertData_<float, short>,
3404       convertData_<float, int>, convertData_<float, float>,
3405       convertData_<float, double>, 0 },
3406
3407     { convertData_<double, uchar>, convertData_<double, schar>,
3408       convertData_<double, ushort>, convertData_<double, short>,
3409       convertData_<double, int>, convertData_<double, float>,
3410       convertData_<double, double>, 0 },
3411
3412     { 0, 0, 0, 0, 0, 0, 0, 0 }};
3413
3414     ConvertData func = tab[CV_MAT_DEPTH(fromType)][CV_MAT_DEPTH(toType)];
3415     CV_Assert( func != 0 );
3416     return func;
3417 }
3418
3419 ConvertScaleData getConvertScaleElem(int fromType, int toType)
3420 {
3421     static ConvertScaleData tab[][8] =
3422     {{ convertScaleData_<uchar, uchar>, convertScaleData_<uchar, schar>,
3423       convertScaleData_<uchar, ushort>, convertScaleData_<uchar, short>,
3424       convertScaleData_<uchar, int>, convertScaleData_<uchar, float>,
3425       convertScaleData_<uchar, double>, 0 },
3426
3427     { convertScaleData_<schar, uchar>, convertScaleData_<schar, schar>,
3428       convertScaleData_<schar, ushort>, convertScaleData_<schar, short>,
3429       convertScaleData_<schar, int>, convertScaleData_<schar, float>,
3430       convertScaleData_<schar, double>, 0 },
3431
3432     { convertScaleData_<ushort, uchar>, convertScaleData_<ushort, schar>,
3433       convertScaleData_<ushort, ushort>, convertScaleData_<ushort, short>,
3434       convertScaleData_<ushort, int>, convertScaleData_<ushort, float>,
3435       convertScaleData_<ushort, double>, 0 },
3436
3437     { convertScaleData_<short, uchar>, convertScaleData_<short, schar>,
3438       convertScaleData_<short, ushort>, convertScaleData_<short, short>,
3439       convertScaleData_<short, int>, convertScaleData_<short, float>,
3440       convertScaleData_<short, double>, 0 },
3441
3442     { convertScaleData_<int, uchar>, convertScaleData_<int, schar>,
3443       convertScaleData_<int, ushort>, convertScaleData_<int, short>,
3444       convertScaleData_<int, int>, convertScaleData_<int, float>,
3445       convertScaleData_<int, double>, 0 },
3446
3447     { convertScaleData_<float, uchar>, convertScaleData_<float, schar>,
3448       convertScaleData_<float, ushort>, convertScaleData_<float, short>,
3449       convertScaleData_<float, int>, convertScaleData_<float, float>,
3450       convertScaleData_<float, double>, 0 },
3451
3452     { convertScaleData_<double, uchar>, convertScaleData_<double, schar>,
3453       convertScaleData_<double, ushort>, convertScaleData_<double, short>,
3454       convertScaleData_<double, int>, convertScaleData_<double, float>,
3455       convertScaleData_<double, double>, 0 },
3456
3457     { 0, 0, 0, 0, 0, 0, 0, 0 }};
3458
3459     ConvertScaleData func = tab[CV_MAT_DEPTH(fromType)][CV_MAT_DEPTH(toType)];
3460     CV_Assert( func != 0 );
3461     return func;
3462 }
3463
3464 enum { HASH_SIZE0 = 8 };
3465
3466 static inline void copyElem(const uchar* from, uchar* to, size_t elemSize)
3467 {
3468     size_t i;
3469     for( i = 0; (int)i <= (int)(elemSize - sizeof(int)); i += sizeof(int) )
3470         *(int*)(to + i) = *(const int*)(from + i);
3471     for( ; i < elemSize; i++ )
3472         to[i] = from[i];
3473 }
3474
3475 static inline bool isZeroElem(const uchar* data, size_t elemSize)
3476 {
3477     size_t i;
3478     for( i = 0; i <= elemSize - sizeof(int); i += sizeof(int) )
3479         if( *(int*)(data + i) != 0 )
3480             return false;
3481     for( ; i < elemSize; i++ )
3482         if( data[i] != 0 )
3483             return false;
3484     return true;
3485 }
3486
3487 SparseMat::Hdr::Hdr( int _dims, const int* _sizes, int _type )
3488 {
3489     refcount = 1;
3490
3491     dims = _dims;
3492     valueOffset = (int)alignSize(sizeof(SparseMat::Node) +
3493         sizeof(int)*std::max(dims - CV_MAX_DIM, 0), CV_ELEM_SIZE1(_type));
3494     nodeSize = alignSize(valueOffset +
3495         CV_ELEM_SIZE(_type), (int)sizeof(size_t));
3496
3497     int i;
3498     for( i = 0; i < dims; i++ )
3499         size[i] = _sizes[i];
3500     for( ; i < CV_MAX_DIM; i++ )
3501         size[i] = 0;
3502     clear();
3503 }
3504
3505 void SparseMat::Hdr::clear()
3506 {
3507     hashtab.clear();
3508     hashtab.resize(HASH_SIZE0);
3509     pool.clear();
3510     pool.resize(nodeSize);
3511     nodeCount = freeList = 0;
3512 }
3513
3514
3515 SparseMat::SparseMat(const Mat& m)
3516 : flags(MAGIC_VAL), hdr(0)
3517 {
3518     create( m.dims, m.size, m.type() );
3519
3520     int i, idx[CV_MAX_DIM] = {0}, d = m.dims, lastSize = m.size[d - 1];
3521     size_t esz = m.elemSize();
3522     uchar* dptr = m.data;
3523
3524     for(;;)
3525     {
3526         for( i = 0; i < lastSize; i++, dptr += esz )
3527         {
3528             if( isZeroElem(dptr, esz) )
3529                 continue;
3530             idx[d-1] = i;
3531             uchar* to = newNode(idx, hash(idx));
3532             copyElem( dptr, to, esz );
3533         }
3534
3535         for( i = d - 2; i >= 0; i-- )
3536         {
3537             dptr += m.step[i] - m.size[i+1]*m.step[i+1];
3538             if( ++idx[i] < m.size[i] )
3539                 break;
3540             idx[i] = 0;
3541         }
3542         if( i < 0 )
3543             break;
3544     }
3545 }
3546
3547 SparseMat::SparseMat(const CvSparseMat* m)
3548 : flags(MAGIC_VAL), hdr(0)
3549 {
3550     CV_Assert(m);
3551     create( m->dims, &m->size[0], m->type );
3552
3553     CvSparseMatIterator it;
3554     CvSparseNode* n = cvInitSparseMatIterator(m, &it);
3555     size_t esz = elemSize();
3556
3557     for( ; n != 0; n = cvGetNextSparseNode(&it) )
3558     {
3559         const int* idx = CV_NODE_IDX(m, n);
3560         uchar* to = newNode(idx, hash(idx));
3561         copyElem((const uchar*)CV_NODE_VAL(m, n), to, esz);
3562     }
3563 }
3564
3565 void SparseMat::create(int d, const int* _sizes, int _type)
3566 {
3567     int i;
3568     CV_Assert( _sizes && 0 < d && d <= CV_MAX_DIM );
3569     for( i = 0; i < d; i++ )
3570         CV_Assert( _sizes[i] > 0 );
3571     _type = CV_MAT_TYPE(_type);
3572     if( hdr && _type == type() && hdr->dims == d && hdr->refcount == 1 )
3573     {
3574         for( i = 0; i < d; i++ )
3575             if( _sizes[i] != hdr->size[i] )
3576                 break;
3577         if( i == d )
3578         {
3579             clear();
3580             return;
3581         }
3582     }
3583     release();
3584     flags = MAGIC_VAL | _type;
3585     hdr = new Hdr(d, _sizes, _type);
3586 }
3587
3588 void SparseMat::copyTo( SparseMat& m ) const
3589 {
3590     if( hdr == m.hdr )
3591         return;
3592     if( !hdr )
3593     {
3594         m.release();
3595         return;
3596     }
3597     m.create( hdr->dims, hdr->size, type() );
3598     SparseMatConstIterator from = begin();
3599     size_t i, N = nzcount(), esz = elemSize();
3600
3601     for( i = 0; i < N; i++, ++from )
3602     {
3603         const Node* n = from.node();
3604         uchar* to = m.newNode(n->idx, n->hashval);
3605         copyElem( from.ptr, to, esz );
3606     }
3607 }
3608
3609 void SparseMat::copyTo( Mat& m ) const
3610 {
3611     CV_Assert( hdr );
3612     m.create( dims(), hdr->size, type() );
3613     m = Scalar(0);
3614
3615     SparseMatConstIterator from = begin();
3616     size_t i, N = nzcount(), esz = elemSize();
3617
3618     for( i = 0; i < N; i++, ++from )
3619     {
3620         const Node* n = from.node();
3621         copyElem( from.ptr, m.ptr(n->idx), esz);
3622     }
3623 }
3624
3625
3626 void SparseMat::convertTo( SparseMat& m, int rtype, double alpha ) const
3627 {
3628     int cn = channels();
3629     if( rtype < 0 )
3630         rtype = type();
3631     rtype = CV_MAKETYPE(rtype, cn);
3632     if( hdr == m.hdr && rtype != type()  )
3633     {
3634         SparseMat temp;
3635         convertTo(temp, rtype, alpha);
3636         m = temp;
3637         return;
3638     }
3639
3640     CV_Assert(hdr != 0);
3641     if( hdr != m.hdr )
3642         m.create( hdr->dims, hdr->size, rtype );
3643
3644     SparseMatConstIterator from = begin();
3645     size_t i, N = nzcount();
3646
3647     if( alpha == 1 )
3648     {
3649         ConvertData cvtfunc = getConvertElem(type(), rtype);
3650         for( i = 0; i < N; i++, ++from )
3651         {
3652             const Node* n = from.node();
3653             uchar* to = hdr == m.hdr ? from.ptr : m.newNode(n->idx, n->hashval);
3654             cvtfunc( from.ptr, to, cn );
3655         }
3656     }
3657     else
3658     {
3659         ConvertScaleData cvtfunc = getConvertScaleElem(type(), rtype);
3660         for( i = 0; i < N; i++, ++from )
3661         {
3662             const Node* n = from.node();
3663             uchar* to = hdr == m.hdr ? from.ptr : m.newNode(n->idx, n->hashval);
3664             cvtfunc( from.ptr, to, cn, alpha, 0 );
3665         }
3666     }
3667 }
3668
3669
3670 void SparseMat::convertTo( Mat& m, int rtype, double alpha, double beta ) const
3671 {
3672     int cn = channels();
3673     if( rtype < 0 )
3674         rtype = type();
3675     rtype = CV_MAKETYPE(rtype, cn);
3676
3677     CV_Assert( hdr );
3678     m.create( dims(), hdr->size, rtype );
3679     m = Scalar(beta);
3680
3681     SparseMatConstIterator from = begin();
3682     size_t i, N = nzcount();
3683
3684     if( alpha == 1 && beta == 0 )
3685     {
3686         ConvertData cvtfunc = getConvertElem(type(), rtype);
3687         for( i = 0; i < N; i++, ++from )
3688         {
3689             const Node* n = from.node();
3690             uchar* to = m.ptr(n->idx);
3691             cvtfunc( from.ptr, to, cn );
3692         }
3693     }
3694     else
3695     {
3696         ConvertScaleData cvtfunc = getConvertScaleElem(type(), rtype);
3697         for( i = 0; i < N; i++, ++from )
3698         {
3699             const Node* n = from.node();
3700             uchar* to = m.ptr(n->idx);
3701             cvtfunc( from.ptr, to, cn, alpha, beta );
3702         }
3703     }
3704 }
3705
3706 void SparseMat::clear()
3707 {
3708     if( hdr )
3709         hdr->clear();
3710 }
3711
3712 SparseMat::operator CvSparseMat*() const
3713 {
3714     if( !hdr )
3715         return 0;
3716     CvSparseMat* m = cvCreateSparseMat(hdr->dims, hdr->size, type());
3717
3718     SparseMatConstIterator from = begin();
3719     size_t i, N = nzcount(), esz = elemSize();
3720
3721     for( i = 0; i < N; i++, ++from )
3722     {
3723         const Node* n = from.node();
3724         uchar* to = cvPtrND(m, n->idx, 0, -2, 0);
3725         copyElem(from.ptr, to, esz);
3726     }
3727     return m;
3728 }
3729
3730 uchar* SparseMat::ptr(int i0, bool createMissing, size_t* hashval)
3731 {
3732     CV_Assert( hdr && hdr->dims == 1 );
3733     size_t h = hashval ? *hashval : hash(i0);
3734     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
3735     uchar* pool = &hdr->pool[0];
3736     while( nidx != 0 )
3737     {
3738         Node* elem = (Node*)(pool + nidx);
3739         if( elem->hashval == h && elem->idx[0] == i0 )
3740             return &value<uchar>(elem);
3741         nidx = elem->next;
3742     }
3743
3744     if( createMissing )
3745     {
3746         int idx[] = { i0 };
3747         return newNode( idx, h );
3748     }
3749     return 0;
3750 }
3751
3752 uchar* SparseMat::ptr(int i0, int i1, bool createMissing, size_t* hashval)
3753 {
3754     CV_Assert( hdr && hdr->dims == 2 );
3755     size_t h = hashval ? *hashval : hash(i0, i1);
3756     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
3757     uchar* pool = &hdr->pool[0];
3758     while( nidx != 0 )
3759     {
3760         Node* elem = (Node*)(pool + nidx);
3761         if( elem->hashval == h && elem->idx[0] == i0 && elem->idx[1] == i1 )
3762             return &value<uchar>(elem);
3763         nidx = elem->next;
3764     }
3765
3766     if( createMissing )
3767     {
3768         int idx[] = { i0, i1 };
3769         return newNode( idx, h );
3770     }
3771     return 0;
3772 }
3773
3774 uchar* SparseMat::ptr(int i0, int i1, int i2, bool createMissing, size_t* hashval)
3775 {
3776     CV_Assert( hdr && hdr->dims == 3 );
3777     size_t h = hashval ? *hashval : hash(i0, i1, i2);
3778     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
3779     uchar* pool = &hdr->pool[0];
3780     while( nidx != 0 )
3781     {
3782         Node* elem = (Node*)(pool + nidx);
3783         if( elem->hashval == h && elem->idx[0] == i0 &&
3784             elem->idx[1] == i1 && elem->idx[2] == i2 )
3785             return &value<uchar>(elem);
3786         nidx = elem->next;
3787     }
3788
3789     if( createMissing )
3790     {
3791         int idx[] = { i0, i1, i2 };
3792         return newNode( idx, h );
3793     }
3794     return 0;
3795 }
3796
3797 uchar* SparseMat::ptr(const int* idx, bool createMissing, size_t* hashval)
3798 {
3799     CV_Assert( hdr );
3800     int i, d = hdr->dims;
3801     size_t h = hashval ? *hashval : hash(idx);
3802     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx];
3803     uchar* pool = &hdr->pool[0];
3804     while( nidx != 0 )
3805     {
3806         Node* elem = (Node*)(pool + nidx);
3807         if( elem->hashval == h )
3808         {
3809             for( i = 0; i < d; i++ )
3810                 if( elem->idx[i] != idx[i] )
3811                     break;
3812             if( i == d )
3813                 return &value<uchar>(elem);
3814         }
3815         nidx = elem->next;
3816     }
3817
3818     return createMissing ? newNode(idx, h) : 0;
3819 }
3820
3821 void SparseMat::erase(int i0, int i1, size_t* hashval)
3822 {
3823     CV_Assert( hdr && hdr->dims == 2 );
3824     size_t h = hashval ? *hashval : hash(i0, i1);
3825     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
3826     uchar* pool = &hdr->pool[0];
3827     while( nidx != 0 )
3828     {
3829         Node* elem = (Node*)(pool + nidx);
3830         if( elem->hashval == h && elem->idx[0] == i0 && elem->idx[1] == i1 )
3831             break;
3832         previdx = nidx;
3833         nidx = elem->next;
3834     }
3835
3836     if( nidx )
3837         removeNode(hidx, nidx, previdx);
3838 }
3839
3840 void SparseMat::erase(int i0, int i1, int i2, size_t* hashval)
3841 {
3842     CV_Assert( hdr && hdr->dims == 3 );
3843     size_t h = hashval ? *hashval : hash(i0, i1, i2);
3844     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
3845     uchar* pool = &hdr->pool[0];
3846     while( nidx != 0 )
3847     {
3848         Node* elem = (Node*)(pool + nidx);
3849         if( elem->hashval == h && elem->idx[0] == i0 &&
3850             elem->idx[1] == i1 && elem->idx[2] == i2 )
3851             break;
3852         previdx = nidx;
3853         nidx = elem->next;
3854     }
3855
3856     if( nidx )
3857         removeNode(hidx, nidx, previdx);
3858 }
3859
3860 void SparseMat::erase(const int* idx, size_t* hashval)
3861 {
3862     CV_Assert( hdr );
3863     int i, d = hdr->dims;
3864     size_t h = hashval ? *hashval : hash(idx);
3865     size_t hidx = h & (hdr->hashtab.size() - 1), nidx = hdr->hashtab[hidx], previdx=0;
3866     uchar* pool = &hdr->pool[0];
3867     while( nidx != 0 )
3868     {
3869         Node* elem = (Node*)(pool + nidx);
3870         if( elem->hashval == h )
3871         {
3872             for( i = 0; i < d; i++ )
3873                 if( elem->idx[i] != idx[i] )
3874                     break;
3875             if( i == d )
3876                 break;
3877         }
3878         previdx = nidx;
3879         nidx = elem->next;
3880     }
3881
3882     if( nidx )
3883         removeNode(hidx, nidx, previdx);
3884 }
3885
3886 void SparseMat::resizeHashTab(size_t newsize)
3887 {
3888     newsize = std::max(newsize, (size_t)8);
3889     if((newsize & (newsize-1)) != 0)
3890         newsize = (size_t)1 << cvCeil(std::log((double)newsize)/CV_LOG2);
3891
3892     size_t i, hsize = hdr->hashtab.size();
3893     vector<size_t> _newh(newsize);
3894     size_t* newh = &_newh[0];
3895     for( i = 0; i < newsize; i++ )
3896         newh[i] = 0;
3897     uchar* pool = &hdr->pool[0];
3898     for( i = 0; i < hsize; i++ )
3899     {
3900         size_t nidx = hdr->hashtab[i];
3901         while( nidx )
3902         {
3903             Node* elem = (Node*)(pool + nidx);
3904             size_t next = elem->next;
3905             size_t newhidx = elem->hashval & (newsize - 1);
3906             elem->next = newh[newhidx];
3907             newh[newhidx] = nidx;
3908             nidx = next;
3909         }
3910     }
3911     hdr->hashtab = _newh;
3912 }
3913
3914 uchar* SparseMat::newNode(const int* idx, size_t hashval)
3915 {
3916     const int HASH_MAX_FILL_FACTOR=3;
3917     assert(hdr);
3918     size_t hsize = hdr->hashtab.size();
3919     if( ++hdr->nodeCount > hsize*HASH_MAX_FILL_FACTOR )
3920     {
3921         resizeHashTab(std::max(hsize*2, (size_t)8));
3922         hsize = hdr->hashtab.size();
3923     }
3924
3925     if( !hdr->freeList )
3926     {
3927         size_t i, nsz = hdr->nodeSize, psize = hdr->pool.size(),
3928             newpsize = std::max(psize*2, 8*nsz);
3929         hdr->pool.resize(newpsize);
3930         uchar* pool = &hdr->pool[0];
3931         hdr->freeList = std::max(psize, nsz);
3932         for( i = hdr->freeList; i < newpsize - nsz; i += nsz )
3933             ((Node*)(pool + i))->next = i + nsz;
3934         ((Node*)(pool + i))->next = 0;
3935     }
3936     size_t nidx = hdr->freeList;
3937     Node* elem = (Node*)&hdr->pool[nidx];
3938     hdr->freeList = elem->next;
3939     elem->hashval = hashval;
3940     size_t hidx = hashval & (hsize - 1);
3941     elem->next = hdr->hashtab[hidx];
3942     hdr->hashtab[hidx] = nidx;
3943
3944     int i, d = hdr->dims;
3945     for( i = 0; i < d; i++ )
3946         elem->idx[i] = idx[i];
3947     size_t esz = elemSize();
3948     uchar* p = &value<uchar>(elem);
3949     if( esz == sizeof(float) )
3950         *((float*)p) = 0.f;
3951     else if( esz == sizeof(double) )
3952         *((double*)p) = 0.;
3953     else
3954         memset(p, 0, esz);
3955
3956     return p;
3957 }
3958
3959
3960 void SparseMat::removeNode(size_t hidx, size_t nidx, size_t previdx)
3961 {
3962     Node* n = node(nidx);
3963     if( previdx )
3964     {
3965         Node* prev = node(previdx);
3966         prev->next = n->next;
3967     }
3968     else
3969         hdr->hashtab[hidx] = n->next;
3970     n->next = hdr->freeList;
3971     hdr->freeList = nidx;
3972     --hdr->nodeCount;
3973 }
3974
3975
3976 SparseMatConstIterator::SparseMatConstIterator(const SparseMat* _m)
3977 : m((SparseMat*)_m), hashidx(0), ptr(0)
3978 {
3979     if(!_m || !_m->hdr)
3980         return;
3981     SparseMat::Hdr& hdr = *m->hdr;
3982     const vector<size_t>& htab = hdr.hashtab;
3983     size_t i, hsize = htab.size();
3984     for( i = 0; i < hsize; i++ )
3985     {
3986         size_t nidx = htab[i];
3987         if( nidx )
3988         {
3989             hashidx = i;
3990             ptr = &hdr.pool[nidx] + hdr.valueOffset;
3991             return;
3992         }
3993     }
3994 }
3995
3996 SparseMatConstIterator& SparseMatConstIterator::operator ++()
3997 {
3998     if( !ptr || !m || !m->hdr )
3999         return *this;
4000     SparseMat::Hdr& hdr = *m->hdr;
4001     size_t next = ((const SparseMat::Node*)(ptr - hdr.valueOffset))->next;
4002     if( next )
4003     {
4004         ptr = &hdr.pool[next] + hdr.valueOffset;
4005         return *this;
4006     }
4007     size_t i = hashidx + 1, sz = hdr.hashtab.size();
4008     for( ; i < sz; i++ )
4009     {
4010         size_t nidx = hdr.hashtab[i];
4011         if( nidx )
4012         {
4013             hashidx = i;
4014             ptr = &hdr.pool[nidx] + hdr.valueOffset;
4015             return *this;
4016         }
4017     }
4018     hashidx = sz;
4019     ptr = 0;
4020     return *this;
4021 }
4022
4023
4024 double norm( const SparseMat& src, int normType )
4025 {
4026     SparseMatConstIterator it = src.begin();
4027
4028     size_t i, N = src.nzcount();
4029     normType &= NORM_TYPE_MASK;
4030     int type = src.type();
4031     double result = 0;
4032
4033     CV_Assert( normType == NORM_INF || normType == NORM_L1 || normType == NORM_L2 );
4034
4035     if( type == CV_32F )
4036     {
4037         if( normType == NORM_INF )
4038             for( i = 0; i < N; i++, ++it )
4039                 result = std::max(result, std::abs((double)*(const float*)it.ptr));
4040         else if( normType == NORM_L1 )
4041             for( i = 0; i < N; i++, ++it )
4042                 result += std::abs(*(const float*)it.ptr);
4043         else
4044             for( i = 0; i < N; i++, ++it )
4045             {
4046                 double v = *(const float*)it.ptr;
4047                 result += v*v;
4048             }
4049     }
4050     else if( type == CV_64F )
4051     {
4052         if( normType == NORM_INF )
4053             for( i = 0; i < N; i++, ++it )
4054                 result = std::max(result, std::abs(*(const double*)it.ptr));
4055         else if( normType == NORM_L1 )
4056             for( i = 0; i < N; i++, ++it )
4057                 result += std::abs(*(const double*)it.ptr);
4058         else
4059             for( i = 0; i < N; i++, ++it )
4060             {
4061                 double v = *(const double*)it.ptr;
4062                 result += v*v;
4063             }
4064     }
4065     else
4066         CV_Error( CV_StsUnsupportedFormat, "Only 32f and 64f are supported" );
4067
4068     if( normType == NORM_L2 )
4069         result = std::sqrt(result);
4070     return result;
4071 }
4072
4073 void minMaxLoc( const SparseMat& src, double* _minval, double* _maxval, int* _minidx, int* _maxidx )
4074 {
4075     SparseMatConstIterator it = src.begin();
4076     size_t i, N = src.nzcount(), d = src.hdr ? src.hdr->dims : 0;
4077     int type = src.type();
4078     const int *minidx = 0, *maxidx = 0;
4079
4080     if( type == CV_32F )
4081     {
4082         float minval = FLT_MAX, maxval = -FLT_MAX;
4083         for( i = 0; i < N; i++, ++it )
4084         {
4085             float v = *(const float*)it.ptr;
4086             if( v < minval )
4087             {
4088                 minval = v;
4089                 minidx = it.node()->idx;
4090             }
4091             if( v > maxval )
4092             {
4093                 maxval = v;
4094                 maxidx = it.node()->idx;
4095             }
4096         }
4097         if( _minval )
4098             *_minval = minval;
4099         if( _maxval )
4100             *_maxval = maxval;
4101     }
4102     else if( type == CV_64F )
4103     {
4104         double minval = DBL_MAX, maxval = -DBL_MAX;
4105         for( i = 0; i < N; i++, ++it )
4106         {
4107             double v = *(const double*)it.ptr;
4108             if( v < minval )
4109             {
4110                 minval = v;
4111                 minidx = it.node()->idx;
4112             }
4113             if( v > maxval )
4114             {
4115                 maxval = v;
4116                 maxidx = it.node()->idx;
4117             }
4118         }
4119         if( _minval )
4120             *_minval = minval;
4121         if( _maxval )
4122             *_maxval = maxval;
4123     }
4124     else
4125         CV_Error( CV_StsUnsupportedFormat, "Only 32f and 64f are supported" );
4126
4127     if( _minidx )
4128         for( i = 0; i < d; i++ )
4129             _minidx[i] = minidx[i];
4130     if( _maxidx )
4131         for( i = 0; i < d; i++ )
4132             _maxidx[i] = maxidx[i];
4133 }
4134
4135
4136 void normalize( const SparseMat& src, SparseMat& dst, double a, int norm_type )
4137 {
4138     double scale = 1;
4139     if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
4140     {
4141         scale = norm( src, norm_type );
4142         scale = scale > DBL_EPSILON ? a/scale : 0.;
4143     }
4144     else
4145         CV_Error( CV_StsBadArg, "Unknown/unsupported norm type" );
4146
4147     src.convertTo( dst, -1, scale );
4148 }
4149
4150 ////////////////////// RotatedRect //////////////////////
4151
4152 void RotatedRect::points(Point2f pt[]) const
4153 {
4154     double _angle = angle*CV_PI/180.;
4155     float b = (float)cos(_angle)*0.5f;
4156     float a = (float)sin(_angle)*0.5f;
4157
4158     pt[0].x = center.x - a*size.height - b*size.width;
4159     pt[0].y = center.y + b*size.height - a*size.width;
4160     pt[1].x = center.x + a*size.height - b*size.width;
4161     pt[1].y = center.y - b*size.height - a*size.width;
4162     pt[2].x = 2*center.x - pt[0].x;
4163     pt[2].y = 2*center.y - pt[0].y;
4164     pt[3].x = 2*center.x - pt[1].x;
4165     pt[3].y = 2*center.y - pt[1].y;
4166 }
4167
4168 Rect RotatedRect::boundingRect() const
4169 {
4170     Point2f pt[4];
4171     points(pt);
4172     Rect r(cvFloor(min(min(min(pt[0].x, pt[1].x), pt[2].x), pt[3].x)),
4173            cvFloor(min(min(min(pt[0].y, pt[1].y), pt[2].y), pt[3].y)),
4174            cvCeil(max(max(max(pt[0].x, pt[1].x), pt[2].x), pt[3].x)),
4175            cvCeil(max(max(max(pt[0].y, pt[1].y), pt[2].y), pt[3].y)));
4176     r.width -= r.x - 1;
4177     r.height -= r.y - 1;
4178     return r;
4179 }
4180
4181 }
4182
4183 /* End of file. */