vcs.maemo.org Git - opencv/blob - include/opencv/cxoperations.hpp

   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 #ifndef _OPENCV_CORE_OPERATIONS_H_
  44 #define _OPENCV_CORE_OPERATIONS_H_
  45
  46 #ifndef SKIP_INCLUDES
  47   #include <string.h>
  48   #include <limits.h>
  49 #endif // SKIP_INCLUDES
  50
  51 #ifdef __cplusplus
  52
  53 /////// exchange-add operation for atomic operations on reference counters ///////
  54 #ifdef __GNUC__
  55
  56   #if __GNUC__*10 + __GNUC_MINOR__ >= 42
  57
  58     #if !defined WIN32 && (defined __i486__ || defined __i586__ || \
  59         defined __i686__ || defined __MMX__ || defined __SSE__  || defined __ppc__)
  60       #define CV_XADD __sync_fetch_and_add
  61     #else
  62       #include <ext/atomicity.h>
  63       #define CV_XADD __gnu_cxx::__exchange_and_add
  64     #endif
  65
  66   #else
  67     #include <bits/atomicity.h>
  68     #if __GNUC__ >= 4
  69       #define CV_XADD __gnu_cxx::__exchange_and_add
  70     #else
  71       #define CV_XADD __exchange_and_add
  72     #endif
  73   #endif
  74
  75 #elif defined WIN32 || defined _WIN32
  76
  77   #if defined _MSC_VER && !defined WIN64 && !defined _WIN64
  78     static inline int CV_XADD( int* addr, int delta )
  79     {
  80         int tmp;
  81         __asm
  82         {
  83             mov edx, addr
  84             mov eax, delta
  85             lock xadd [edx], eax
  86             mov tmp, eax
  87         }
  88         return tmp;
  89     }
  90   #else
  91     #include "windows.h"
  92     #undef min
  93     #undef max
  94     #define CV_XADD(addr,delta) InterlockedExchangeAdd((LONG volatile*)(addr), (delta))
  95   #endif
  96
  97 #else
  98
  99   template<typename _Tp> static inline _Tp CV_XADD(_Tp* addr, _Tp delta)
 100   { int tmp = *addr; *addr += delta; return tmp; }
 101
 102 #endif
 103
 104
 105 namespace cv
 106 {
 107
 108 using std::cos;
 109 using std::sin;
 110 using std::max;
 111 using std::min;
 112 using std::exp;
 113 using std::log;
 114 using std::pow;
 115 using std::sqrt;
 116
 117
 118 /////////////// saturate_cast (used in image & signal processing) ///////////////////
 119
 120 template<typename _Tp> static inline _Tp saturate_cast(uchar v) { return _Tp(v); }
 121 template<typename _Tp> static inline _Tp saturate_cast(schar v) { return _Tp(v); }
 122 template<typename _Tp> static inline _Tp saturate_cast(ushort v) { return _Tp(v); }
 123 template<typename _Tp> static inline _Tp saturate_cast(short v) { return _Tp(v); }
 124 template<typename _Tp> static inline _Tp saturate_cast(unsigned v) { return _Tp(v); }
 125 template<typename _Tp> static inline _Tp saturate_cast(int v) { return _Tp(v); }
 126 template<typename _Tp> static inline _Tp saturate_cast(float v) { return _Tp(v); }
 127 template<typename _Tp> static inline _Tp saturate_cast(double v) { return _Tp(v); }
 128
 129 template<> inline uchar saturate_cast<uchar>(schar v)
 130 { return (uchar)std::max((int)v, 0); }
 131 template<> inline uchar saturate_cast<uchar>(ushort v)
 132 { return (uchar)std::min((unsigned)v, (unsigned)UCHAR_MAX); }
 133 template<> inline uchar saturate_cast<uchar>(int v)
 134 { return (uchar)((unsigned)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0); }
 135 template<> inline uchar saturate_cast<uchar>(short v)
 136 { return saturate_cast<uchar>((int)v); }
 137 template<> inline uchar saturate_cast<uchar>(unsigned v)
 138 { return (uchar)std::min(v, (unsigned)UCHAR_MAX); }
 139 template<> inline uchar saturate_cast<uchar>(float v)
 140 { int iv = cvRound(v); return saturate_cast<uchar>(iv); }
 141 template<> inline uchar saturate_cast<uchar>(double v)
 142 { int iv = cvRound(v); return saturate_cast<uchar>(iv); }
 143
 144 template<> inline schar saturate_cast<schar>(uchar v)
 145 { return (schar)std::min((int)v, SCHAR_MAX); }
 146 template<> inline schar saturate_cast<schar>(ushort v)
 147 { return (schar)std::min((unsigned)v, (unsigned)SCHAR_MAX); }
 148 template<> inline schar saturate_cast<schar>(int v)
 149 {
 150     return (schar)((unsigned)(v-SCHAR_MIN) <= (unsigned)UCHAR_MAX ?
 151                 v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
 152 }
 153 template<> inline schar saturate_cast<schar>(short v)
 154 { return saturate_cast<schar>((int)v); }
 155 template<> inline schar saturate_cast<schar>(unsigned v)
 156 { return (schar)std::min(v, (unsigned)SCHAR_MAX); }
 157
 158 template<> inline schar saturate_cast<schar>(float v)
 159 { int iv = cvRound(v); return saturate_cast<schar>(iv); }
 160 template<> inline schar saturate_cast<schar>(double v)
 161 { int iv = cvRound(v); return saturate_cast<schar>(iv); }
 162
 163 template<> inline ushort saturate_cast<ushort>(schar v)
 164 { return (ushort)std::max((int)v, 0); }
 165 template<> inline ushort saturate_cast<ushort>(short v)
 166 { return (ushort)std::max((int)v, 0); }
 167 template<> inline ushort saturate_cast<ushort>(int v)
 168 { return (ushort)((unsigned)v <= (unsigned)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0); }
 169 template<> inline ushort saturate_cast<ushort>(unsigned v)
 170 { return (ushort)std::min(v, (unsigned)USHRT_MAX); }
 171 template<> inline ushort saturate_cast<ushort>(float v)
 172 { int iv = cvRound(v); return saturate_cast<ushort>(iv); }
 173 template<> inline ushort saturate_cast<ushort>(double v)
 174 { int iv = cvRound(v); return saturate_cast<ushort>(iv); }
 175
 176 template<> inline short saturate_cast<short>(ushort v)
 177 { return (short)std::min((int)v, SHRT_MAX); }
 178 template<> inline short saturate_cast<short>(int v)
 179 {
 180     return (short)((unsigned)(v - SHRT_MIN) <= (unsigned)USHRT_MAX ?
 181             v : v > 0 ? SHRT_MAX : SHRT_MIN);
 182 }
 183 template<> inline short saturate_cast<short>(unsigned v)
 184 { return (short)std::min(v, (unsigned)SHRT_MAX); }
 185 template<> inline short saturate_cast<short>(float v)
 186 { int iv = cvRound(v); return saturate_cast<short>(iv); }
 187 template<> inline short saturate_cast<short>(double v)
 188 { int iv = cvRound(v); return saturate_cast<short>(iv); }
 189
 190 template<> inline int saturate_cast<int>(float v) { return cvRound(v); }
 191 template<> inline int saturate_cast<int>(double v) { return cvRound(v); }
 192
 193 // we intentionally do not clip negative numbers, to make -1 become 0xffffffff etc.
 194 template<> inline unsigned saturate_cast<unsigned>(float v){ return cvRound(v); }
 195 template<> inline unsigned saturate_cast<unsigned>(double v) { return cvRound(v); }
 196
 197
 198 /////////////////////////// short vector (Vec) /////////////////////////////
 199
 200 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec() {}
 201 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0)
 202 {
 203     val[0] = v0;
 204     for(int i = 1; i < cn; i++) val[i] = _Tp();
 205 }
 206
 207 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1)
 208 {
 209     assert(cn >= 2);
 210     val[0] = v0; val[1] = v1;
 211     for(int i = 2; i < cn; i++) val[i] = _Tp(0);
 212 }
 213
 214 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2)
 215 {
 216     assert(cn >= 3);
 217     val[0] = v0; val[1] = v1; val[2] = v2;
 218     for(int i = 3; i < cn; i++) val[i] = _Tp(0);
 219 }
 220
 221 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2, _Tp v3)
 222 {
 223     assert(cn >= 4);
 224     val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3;
 225     for(int i = 4; i < cn; i++) val[i] = _Tp(0);
 226 }
 227
 228 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2, _Tp v3, _Tp v4)
 229 {
 230     assert(cn >= 5);
 231     val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3; val[4] = v4;
 232     for(int i = 5; i < cn; i++) val[i] = _Tp(0);
 233 }
 234
 235 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2, _Tp v3,
 236                                                         _Tp v4, _Tp v5)
 237 {
 238     assert(cn >= 6);
 239     val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3;
 240     val[4] = v4; val[5] = v5;
 241     for(int i = 6; i < cn; i++) val[i] = _Tp(0);
 242 }
 243
 244 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2, _Tp v3,
 245                                                         _Tp v4, _Tp v5, _Tp v6)
 246 {
 247     assert(cn >= 7);
 248     val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3;
 249     val[4] = v4; val[5] = v5; val[6] = v6;
 250     for(int i = 7; i < cn; i++) val[i] = _Tp(0);
 251 }
 252
 253 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2, _Tp v3,
 254                                                         _Tp v4, _Tp v5, _Tp v6, _Tp v7)
 255 {
 256     assert(cn >= 8);
 257     val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3;
 258     val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7;
 259     for(int i = 8; i < cn; i++) val[i] = _Tp(0);
 260 }
 261
 262 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2, _Tp v3,
 263                                                         _Tp v4, _Tp v5, _Tp v6, _Tp v7,
 264                                                         _Tp v8)
 265 {
 266     assert(cn >= 9);
 267     val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3;
 268     val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7;
 269     val[8] = v8;
 270     for(int i = 9; i < cn; i++) val[i] = _Tp(0);
 271 }
 272
 273 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(_Tp v0, _Tp v1, _Tp v2, _Tp v3,
 274                                                         _Tp v4, _Tp v5, _Tp v6, _Tp v7,
 275                                                         _Tp v8, _Tp v9)
 276 {
 277     assert(cn >= 10);
 278     val[0] = v0; val[1] = v1; val[2] = v2; val[3] = v3;
 279     val[4] = v4; val[5] = v5; val[6] = v6; val[7] = v7;
 280     val[8] = v8; val[9] = v9;
 281     for(int i = 10; i < cn; i++) val[i] = _Tp(0);
 282 }
 283
 284
 285 template<typename _Tp, int cn> inline Vec<_Tp, cn>::Vec(const Vec<_Tp, cn>& v)
 286 {
 287     for( int i = 0; i < cn; i++ ) val[i] = v.val[i];
 288 }
 289
 290 template<typename _Tp, int cn> inline Vec<_Tp, cn> Vec<_Tp, cn>::all(_Tp alpha)
 291 {
 292     Vec v;
 293     for( int i = 0; i < cn; i++ ) v.val[i] = alpha;
 294     return v;
 295 }
 296
 297 template<typename _Tp, int cn> inline _Tp Vec<_Tp, cn>::dot(const Vec<_Tp, cn>& v) const
 298 {
 299     _Tp s = 0;
 300     for( int i = 0; i < cn; i++ ) s += val[i]*v.val[i];
 301     return s;
 302 }
 303
 304 template<typename _Tp, int cn> inline double Vec<_Tp, cn>::ddot(const Vec<_Tp, cn>& v) const
 305 {
 306     double s = 0;
 307     for( int i = 0; i < cn; i++ ) s += (double)val[i]*v.val[i];
 308     return s;
 309 }
 310
 311 template<typename _Tp, int cn> inline Vec<_Tp, cn> Vec<_Tp, cn>::cross(const Vec<_Tp, cn>& v) const
 312 {
 313     return Vec<_Tp, cn>(); // for arbitrary-size vector there is no cross-product defined
 314 }
 315
 316 template<typename _Tp, int cn> template<typename T2>
 317 inline Vec<_Tp, cn>::operator Vec<T2, cn>() const
 318 {
 319     Vec<T2, cn> v;
 320     for( int i = 0; i < cn; i++ ) v.val[i] = saturate_cast<T2>(val[i]);
 321     return v;
 322 }
 323
 324 template<typename _Tp, int cn> inline Vec<_Tp, cn>::operator CvScalar() const
 325 {
 326     CvScalar s = {{0,0,0,0}};
 327     int i;
 328     for( i = 0; i < std::min(cn, 4); i++ ) s.val[i] = val[i];
 329     for( ; i < 4; i++ ) s.val[i] = 0;
 330     return s;
 331 }
 332
 333 template<typename _Tp, int cn> inline _Tp Vec<_Tp, cn>::operator [](int i) const { return val[i]; }
 334 template<typename _Tp, int cn> inline _Tp& Vec<_Tp, cn>::operator[](int i) { return val[i]; }
 335
 336 template<typename _Tp1, typename _Tp2, int cn> static inline Vec<_Tp1, cn>&
 337 operator += (Vec<_Tp1, cn>& a, const Vec<_Tp2, cn>& b)
 338 {
 339     for( int i = 0; i < cn; i++ )
 340         a.val[i] = saturate_cast<_Tp1>(a.val[i] + b.val[i]);
 341     return a;
 342 }
 343
 344 template<typename _Tp1, typename _Tp2, int cn> static inline Vec<_Tp1, cn>&
 345 operator -= (Vec<_Tp1, cn>& a, const Vec<_Tp2, cn>& b)
 346 {
 347     for( int i = 0; i < cn; i++ )
 348         a.val[i] = saturate_cast<_Tp1>(a.val[i] - b.val[i]);
 349     return a;
 350 }
 351
 352 template<typename _Tp, int cn> static inline Vec<_Tp, cn>
 353 operator + (const Vec<_Tp, cn>& a, const Vec<_Tp, cn>& b)
 354 {
 355     Vec<_Tp, cn> c = a;
 356     return c += b;
 357 }
 358
 359 template<typename _Tp, int cn> static inline Vec<_Tp, cn>
 360 operator - (const Vec<_Tp, cn>& a, const Vec<_Tp, cn>& b)
 361 {
 362     Vec<_Tp, cn> c = a;
 363     return c -= b;
 364 }
 365
 366 template<typename _Tp> static inline
 367 Vec<_Tp, 2>& operator *= (Vec<_Tp, 2>& a, _Tp alpha)
 368 {
 369     a[0] *= alpha; a[1] *= alpha;
 370     return a;
 371 }
 372
 373 template<typename _Tp> static inline
 374 Vec<_Tp, 3>& operator *= (Vec<_Tp, 3>& a, _Tp alpha)
 375 {
 376     a[0] *= alpha; a[1] *= alpha; a[2] *= alpha;
 377     return a;
 378 }
 379
 380 template<typename _Tp> static inline
 381 Vec<_Tp, 4>& operator *= (Vec<_Tp, 4>& a, _Tp alpha)
 382 {
 383     a[0] *= alpha; a[1] *= alpha; a[2] *= alpha; a[3] *= alpha;
 384     return a;
 385 }
 386
 387 template<typename _Tp, int cn> static inline Vec<_Tp, cn>
 388 operator * (const Vec<_Tp, cn>& a, _Tp alpha)
 389 {
 390     Vec<_Tp, cn> c = a;
 391     return c *= alpha;
 392 }
 393
 394 template<typename _Tp, int cn> static inline Vec<_Tp, cn>
 395 operator * (_Tp alpha, const Vec<_Tp, cn>& a)
 396 {
 397     return a * alpha;
 398 }
 399
 400 template<typename _Tp, int cn> static inline Vec<_Tp, cn>
 401 operator - (const Vec<_Tp, cn>& a)
 402 {
 403     Vec<_Tp,cn> t;
 404     for( int i = 0; i < cn; i++ ) t.val[i] = saturate_cast<_Tp>(-a.val[i]);
 405     return t;
 406 }
 407
 408 template<> inline Vec<float, 3> Vec<float, 3>::cross(const Vec<float, 3>& v) const
 409 {
 410     return Vec<float,3>(val[1]*v.val[2] - val[2]*v.val[1],
 411                      val[2]*v.val[0] - val[0]*v.val[2],
 412                      val[0]*v.val[1] - val[1]*v.val[0]);
 413 }
 414
 415 template<> inline Vec<double, 3> Vec<double, 3>::cross(const Vec<double, 3>& v) const
 416 {
 417     return Vec<double,3>(val[1]*v.val[2] - val[2]*v.val[1],
 418                      val[2]*v.val[0] - val[0]*v.val[2],
 419                      val[0]*v.val[1] - val[1]*v.val[0]);
 420 }
 421
 422 template<typename T1, typename T2> static inline
 423 Vec<T1, 2>& operator += (Vec<T1, 2>& a, const Vec<T2, 2>& b)
 424 {
 425     a[0] = saturate_cast<T1>(a[0] + b[0]);
 426     a[1] = saturate_cast<T1>(a[1] + b[1]);
 427     return a;
 428 }
 429
 430 template<typename T1, typename T2> static inline
 431 Vec<T1, 3>& operator += (Vec<T1, 3>& a, const Vec<T2, 3>& b)
 432 {
 433     a[0] = saturate_cast<T1>(a[0] + b[0]);
 434     a[1] = saturate_cast<T1>(a[1] + b[1]);
 435     a[2] = saturate_cast<T1>(a[2] + b[2]);
 436     return a;
 437 }
 438
 439 template<typename T1, typename T2> static inline
 440 Vec<T1, 4>& operator += (Vec<T1, 4>& a, const Vec<T2, 4>& b)
 441 {
 442     a[0] = saturate_cast<T1>(a[0] + b[0]);
 443     a[1] = saturate_cast<T1>(a[1] + b[1]);
 444     a[2] = saturate_cast<T1>(a[2] + b[2]);
 445     a[3] = saturate_cast<T1>(a[3] + b[3]);
 446     return a;
 447 }
 448
 449 template<typename T1, int n> static inline
 450 double norm(const Vec<T1, n>& a)
 451 {
 452     double s = 0;
 453     for( int i = 0; i < n; i++ )
 454         s += (double)a.val[i]*a.val[i];
 455     return std::sqrt(s);
 456 }
 457
 458 template<typename T1, int n> static inline
 459 bool operator == (const Vec<T1, n>& a, const Vec<T1, n>& b)
 460 {
 461     for( int i = 0; i < n; i++ )
 462         if( a[i] != b[i] ) return false;
 463     return true;
 464 }
 465
 466 template<typename T1, int n> static inline
 467 bool operator != (const Vec<T1, n>& a, const Vec<T1, n>& b)
 468 {
 469     return !(a == b);
 470 }
 471
 472 //////////////////////////////// Complex //////////////////////////////
 473
 474 template<typename _Tp> inline Complex<_Tp>::Complex() : re(0), im(0) {}
 475 template<typename _Tp> inline Complex<_Tp>::Complex( _Tp _re, _Tp _im ) : re(_re), im(_im) {}
 476 template<typename _Tp> template<typename T2> inline Complex<_Tp>::operator Complex<T2>() const
 477 { return Complex<T2>(saturate_cast<T2>(re), saturate_cast<T2>(im)); }
 478 template<typename _Tp> inline Complex<_Tp> Complex<_Tp>::conj() const
 479 { return Complex<_Tp>(re, -im); }
 480
 481 template<typename _Tp> static inline
 482 bool operator == (const Complex<_Tp>& a, const Complex<_Tp>& b)
 483 { return a.re == b.re && a.im == b.im; }
 484
 485 template<typename _Tp> static inline
 486 Complex<_Tp> operator + (const Complex<_Tp>& a, const Complex<_Tp>& b)
 487 { return Complex<_Tp>( a.re + b.re, a.im + b.im ); }
 488
 489 template<typename _Tp> static inline
 490 Complex<_Tp>& operator += (Complex<_Tp>& a, const Complex<_Tp>& b)
 491 { a.re += b.re; a.im += b.im; return a; }
 492
 493 template<typename _Tp> static inline
 494 Complex<_Tp> operator - (const Complex<_Tp>& a, const Complex<_Tp>& b)
 495 { return Complex<_Tp>( a.re - b.re, a.im - b.im ); }
 496
 497 template<typename _Tp> static inline
 498 Complex<_Tp>& operator -= (Complex<_Tp>& a, const Complex<_Tp>& b)
 499 { a.re -= b.re; a.im -= b.im; return a; }
 500
 501 template<typename _Tp> static inline
 502 Complex<_Tp> operator - (const Complex<_Tp>& a)
 503 { return Complex<_Tp>(-a.re, -a.im); }
 504
 505 template<typename _Tp> static inline
 506 Complex<_Tp> operator * (const Complex<_Tp>& a, const Complex<_Tp>& b)
 507 { return Complex<_Tp>( a.re*b.re - a.im*b.im, a.re*b.im + a.im*b.re ); }
 508
 509 template<typename _Tp> static inline
 510 Complex<_Tp> operator * (const Complex<_Tp>& a, _Tp b)
 511 { return Complex<_Tp>( a.re*b, a.im*b ); }
 512
 513 template<typename _Tp> static inline
 514 Complex<_Tp> operator * (_Tp b, const Complex<_Tp>& a)
 515 { return Complex<_Tp>( a.re*b, a.im*b ); }
 516
 517 template<typename _Tp> static inline
 518 Complex<_Tp> operator + (const Complex<_Tp>& a, _Tp b)
 519 { return Complex<_Tp>( a.re + b, a.im ); }
 520
 521 template<typename _Tp> static inline
 522 Complex<_Tp> operator - (const Complex<_Tp>& a, _Tp b)
 523 { return Complex<_Tp>( a.re - b, a.im ); }
 524
 525 template<typename _Tp> static inline
 526 Complex<_Tp> operator + (_Tp b, const Complex<_Tp>& a)
 527 { return Complex<_Tp>( a.re + b, a.im ); }
 528
 529 template<typename _Tp> static inline
 530 Complex<_Tp> operator - (_Tp b, const Complex<_Tp>& a)
 531 { return Complex<_Tp>( b - a.re, -a.im ); }
 532
 533 template<typename _Tp> static inline
 534 Complex<_Tp>& operator += (Complex<_Tp>& a, _Tp b)
 535 { a.re += b; return a; }
 536
 537 template<typename _Tp> static inline
 538 Complex<_Tp>& operator -= (Complex<_Tp>& a, _Tp b)
 539 { a.re -= b; return a; }
 540
 541 template<typename _Tp> static inline
 542 Complex<_Tp>& operator *= (Complex<_Tp>& a, _Tp b)
 543 { a.re *= b; a.im *= b; return a; }
 544
 545 template<typename _Tp> static inline
 546 double abs(const Complex<_Tp>& a)
 547 { return std::sqrt( (double)a.re*a.re + (double)a.im*a.im); }
 548
 549 template<typename _Tp> static inline
 550 Complex<_Tp> operator / (const Complex<_Tp>& a, const Complex<_Tp>& b)
 551 {
 552     double t = 1./((double)b.re*b.re + (double)b.im*b.im);
 553     return Complex<_Tp>( (_Tp)((a.re*b.re + a.im*b.im)*t),
 554                         (_Tp)((-a.re*b.im + a.im*b.re)*t) );
 555 }
 556
 557 template<typename _Tp> static inline
 558 Complex<_Tp>& operator /= (Complex<_Tp>& a, const Complex<_Tp>& b)
 559 {
 560     return (a = a / b);
 561 }
 562
 563 template<typename _Tp> static inline
 564 Complex<_Tp> operator / (const Complex<_Tp>& a, _Tp b)
 565 {
 566     _Tp t = (_Tp)1/b;
 567     return Complex<_Tp>( a.re*t, a.im*t );
 568 }
 569
 570 template<typename _Tp> static inline
 571 Complex<_Tp> operator / (_Tp b, const Complex<_Tp>& a)
 572 {
 573     return Complex<_Tp>(b)/a;
 574 }
 575
 576 template<typename _Tp> static inline
 577 Complex<_Tp> operator /= (const Complex<_Tp>& a, _Tp b)
 578 {
 579     _Tp t = (_Tp)1/b;
 580     a.re *= t; a.im *= t; return a;
 581 }
 582
 583 //////////////////////////////// 2D Point ////////////////////////////////
 584
 585 template<typename _Tp> inline Point_<_Tp>::Point_() : x(0), y(0) {}
 586 template<typename _Tp> inline Point_<_Tp>::Point_(_Tp _x, _Tp _y) : x(_x), y(_y) {}
 587 template<typename _Tp> inline Point_<_Tp>::Point_(const Point_& pt) : x(pt.x), y(pt.y) {}
 588 template<typename _Tp> inline Point_<_Tp>::Point_(const CvPoint& pt) : x((_Tp)pt.x), y((_Tp)pt.y) {}
 589 template<typename _Tp> inline Point_<_Tp>::Point_(const CvPoint2D32f& pt)
 590     : x(saturate_cast<_Tp>(pt.x)), y(saturate_cast<_Tp>(pt.y)) {}
 591 template<typename _Tp> inline Point_<_Tp>::Point_(const Size_<_Tp>& sz) : x(sz.width), y(sz.height) {}
 592 template<typename _Tp> inline Point_<_Tp>::Point_(const Vec<_Tp,2>& v) : x(v[0]), y(v[1]) {}
 593 template<typename _Tp> inline Point_<_Tp>& Point_<_Tp>::operator = (const Point_& pt)
 594 { x = pt.x; y = pt.y; return *this; }
 595
 596 template<typename _Tp> template<typename _Tp2> inline Point_<_Tp>::operator Point_<_Tp2>() const
 597 { return Point_<_Tp2>(saturate_cast<_Tp2>(x), saturate_cast<_Tp2>(y)); }
 598 template<typename _Tp> inline Point_<_Tp>::operator CvPoint() const
 599 { return cvPoint(saturate_cast<int>(x), saturate_cast<int>(y)); }
 600 template<typename _Tp> inline Point_<_Tp>::operator CvPoint2D32f() const
 601 { return cvPoint2D32f((float)x, (float)y); }
 602 template<typename _Tp> inline Point_<_Tp>::operator Vec<_Tp, 2>() const
 603 { return Vec<_Tp, 2>(x, y); }
 604
 605 template<typename _Tp> inline _Tp Point_<_Tp>::dot(const Point_& pt) const
 606 { return x*pt.x + y*pt.y; }
 607 template<typename _Tp> inline double Point_<_Tp>::ddot(const Point_& pt) const
 608 { return (double)x*pt.x + (double)y*pt.y; }
 609
 610 template<typename _Tp> static inline Point_<_Tp>&
 611 operator += (Point_<_Tp>& a, const Point_<_Tp>& b) { a.x += b.x; a.y += b.y; return a; }
 612
 613 template<typename _Tp> static inline Point_<_Tp>&
 614 operator -= (Point_<_Tp>& a, const Point_<_Tp>& b) { a.x -= b.x; a.y -= b.y; return a; }
 615
 616 template<typename _Tp> static inline Point_<_Tp>&
 617 operator *= (Point_<_Tp>& a, _Tp b) { a.x *= b; a.y *= b; return a; }
 618
 619 template<typename _Tp> static inline double norm(const Point_<_Tp>& pt)
 620 { return std::sqrt((double)pt.x*pt.x + (double)pt.y*pt.y); }
 621
 622 template<typename _Tp> static inline bool operator == (const Point_<_Tp>& a, const Point_<_Tp>& b)
 623 { return a.x == b.x && a.y == b.y; }
 624
 625 template<typename _Tp> static inline bool operator != (const Point_<_Tp>& a, const Point_<_Tp>& b)
 626 { return !(a == b); }
 627
 628 template<typename _Tp> static inline Point_<_Tp> operator + (const Point_<_Tp>& a, const Point_<_Tp>& b)
 629 { return Point_<_Tp>( a.x + b.x, a.y + b.y ); }
 630
 631 template<typename _Tp> static inline Point_<_Tp> operator - (const Point_<_Tp>& a, const Point_<_Tp>& b)
 632 { return Point_<_Tp>( a.x - b.x, a.y - b.y ); }
 633
 634 template<typename _Tp> static inline Point_<_Tp> operator - (const Point_<_Tp>& a)
 635 { return Point_<_Tp>( -a.x, -a.y ); }
 636
 637 template<typename _Tp> static inline Point_<_Tp> operator * (const Point_<_Tp>& a, _Tp b)
 638 { return Point_<_Tp>( a.x*b, a.y*b ); }
 639
 640 template<typename _Tp> static inline Point_<_Tp> operator * (_Tp a, const Point_<_Tp>& b)
 641 { return Point_<_Tp>( a*b.x, a*b.y ); }
 642
 643 //////////////////////////////// 3D Point ////////////////////////////////
 644
 645 template<typename _Tp> inline Point3_<_Tp>::Point3_() : x(0), y(0), z(0) {}
 646 template<typename _Tp> inline Point3_<_Tp>::Point3_(_Tp _x, _Tp _y, _Tp _z) : x(_x), y(_y), z(_z) {}
 647 template<typename _Tp> inline Point3_<_Tp>::Point3_(const Point3_& pt) : x(pt.x), y(pt.y), z(pt.z) {}
 648 template<typename _Tp> inline Point3_<_Tp>::Point3_(const Point_<_Tp>& pt) : x(pt.x), y(pt.y), z(_Tp()) {}
 649 template<typename _Tp> inline Point3_<_Tp>::Point3_(const CvPoint3D32f& pt) :
 650     x(saturate_cast<_Tp>(pt.x)), y(saturate_cast<_Tp>(pt.y)), z(saturate_cast<_Tp>(pt.z)) {}
 651 template<typename _Tp> inline Point3_<_Tp>::Point3_(const Vec<_Tp, 3>& v) : x(v[0]), y(v[1]), z(v[2]) {}
 652
 653 template<typename _Tp> template<typename _Tp2> inline Point3_<_Tp>::operator Point3_<_Tp2>() const
 654 { return Point3_<_Tp2>(saturate_cast<_Tp2>(x), saturate_cast<_Tp2>(y), saturate_cast<_Tp2>(z)); }
 655
 656 template<typename _Tp> inline Point3_<_Tp>::operator CvPoint3D32f() const
 657 { return cvPoint3D32f((float)x, (float)y, (float)z); }
 658
 659 template<typename _Tp> inline Point3_<_Tp>::operator Vec<_Tp, 3>() const
 660 { return Vec<_Tp, 3>(x, y, z); }
 661
 662 template<typename _Tp> inline Point3_<_Tp>& Point3_<_Tp>::operator = (const Point3_& pt)
 663 { x = pt.x; y = pt.y; z = pt.z; return *this; }
 664
 665 template<typename _Tp> inline _Tp Point3_<_Tp>::dot(const Point3_& pt) const
 666 { return x*pt.x + y*pt.y + z*pt.z; }
 667 template<typename _Tp> inline double Point3_<_Tp>::ddot(const Point3_& pt) const
 668 { return (double)x*pt.x + (double)y*pt.y + (double)z*pt.z; }
 669
 670 template<typename _Tp> static inline Point3_<_Tp>&
 671 operator += (Point3_<_Tp>& a, const Point3_<_Tp>& b) { a.x += b.x; a.y += b.y; a.z += b.z; return a; }
 672 template<typename _Tp> static inline Point3_<_Tp>&
 673 operator -= (Point3_<_Tp>& a, const Point3_<_Tp>& b) { a.x -= b.x; a.y -= b.y; a.z -= b.z; return a; }
 674
 675 template<typename _Tp> static inline double norm(const Point3_<_Tp>& pt)
 676 { return std::sqrt((double)pt.x*pt.x + (double)pt.y*pt.y + (double)pt.z*pt.z); }
 677
 678 template<typename _Tp> static inline bool operator == (const Point3_<_Tp>& a, const Point3_<_Tp>& b)
 679 { return a.x == b.x && a.y == b.y && a.z == b.z; }
 680
 681 template<typename _Tp> static inline Point3_<_Tp> operator + (const Point3_<_Tp>& a, const Point3_<_Tp>& b)
 682 { return Point3_<_Tp>( a.x + b.x, a.y + b.y, a.z + b.z ); }
 683
 684 template<typename _Tp> static inline Point3_<_Tp> operator - (const Point3_<_Tp>& a, const Point3_<_Tp>& b)
 685 { return Point3_<_Tp>( a.x - b.x, a.y - b.y, a.z - b.z ); }
 686
 687 template<typename _Tp> static inline Point3_<_Tp> operator - (const Point3_<_Tp>& a)
 688 { return Point3_<_Tp>( -a.x, -a.y, -a.z ); }
 689
 690 template<typename _Tp> static inline Point3_<_Tp> operator * (const Point3_<_Tp>& a, _Tp b)
 691 { return Point3_<_Tp>( a.x*b, a.y*b, a.z*b ); }
 692
 693 template<typename _Tp> static inline Point3_<_Tp> operator * (_Tp a, const Point3_<_Tp>& b)
 694 { return Point3_<_Tp>( a*b.x, a*b.y, a*b.z ); }
 695
 696 //////////////////////////////// Size ////////////////////////////////
 697
 698 template<typename _Tp> inline Size_<_Tp>::Size_()
 699     : width(0), height(0) {}
 700 template<typename _Tp> inline Size_<_Tp>::Size_(_Tp _width, _Tp _height)
 701     : width(_width), height(_height) {}
 702 template<typename _Tp> inline Size_<_Tp>::Size_(const Size_& sz)
 703     : width(sz.width), height(sz.height) {}
 704 template<typename _Tp> inline Size_<_Tp>::Size_(const CvSize& sz)
 705     : width(saturate_cast<_Tp>(sz.width)), height(saturate_cast<_Tp>(sz.height)) {}
 706 template<typename _Tp> inline Size_<_Tp>::Size_(const CvSize2D32f& sz)
 707     : width(saturate_cast<_Tp>(sz.width)), height(saturate_cast<_Tp>(sz.height)) {}
 708 template<typename _Tp> inline Size_<_Tp>::Size_(const Point_<_Tp>& pt) : width(pt.x), height(pt.y) {}
 709
 710 template<typename _Tp> template<typename _Tp2> inline Size_<_Tp>::operator Size_<_Tp2>() const
 711 { return Size_<_Tp2>(saturate_cast<_Tp2>(width), saturate_cast<_Tp2>(height)); }
 712 template<typename _Tp> inline Size_<_Tp>::operator CvSize() const
 713 { return cvSize(saturate_cast<int>(width), saturate_cast<int>(height)); }
 714 template<typename _Tp> inline Size_<_Tp>::operator CvSize2D32f() const
 715 { return cvSize2D32f((float)width, (float)height); }
 716
 717 template<typename _Tp> inline Size_<_Tp>& Size_<_Tp>::operator = (const Size_<_Tp>& sz)
 718 { width = sz.width; height = sz.height; return *this; }
 719 template<typename _Tp> static inline Size_<_Tp> operator * (const Size_<_Tp>& a, _Tp b)
 720 { return Size_<_Tp>(a.width * b, a.height * b); }
 721 template<typename _Tp> static inline Size_<_Tp> operator + (const Size_<_Tp>& a, const Size_<_Tp>& b)
 722 { return Size_<_Tp>(a.width + b.width, a.height + b.height); }
 723 template<typename _Tp> static inline Size_<_Tp> operator - (const Size_<_Tp>& a, const Size_<_Tp>& b)
 724 { return Size_<_Tp>(a.width - b.width, a.height - b.height); }
 725 template<typename _Tp> inline _Tp Size_<_Tp>::area() const { return width*height; }
 726
 727 template<typename _Tp> static inline Size_<_Tp>& operator += (Size_<_Tp>& a, const Size_<_Tp>& b)
 728 { a.width += b.width; a.height += b.height; return a; }
 729 template<typename _Tp> static inline Size_<_Tp>& operator -= (Size_<_Tp>& a, const Size_<_Tp>& b)
 730 { a.width -= b.width; a.height -= b.height; return a; }
 731
 732 template<typename _Tp> static inline bool operator == (const Size_<_Tp>& a, const Size_<_Tp>& b)
 733 { return a.width == b.width && a.height == b.height; }
 734 template<typename _Tp> static inline bool operator != (const Size_<_Tp>& a, const Size_<_Tp>& b)
 735 { return a.width != b.width || a.height != b.height; }
 736
 737 //////////////////////////////// Rect ////////////////////////////////
 738
 739
 740 template<typename _Tp> inline Rect_<_Tp>::Rect_() : x(0), y(0), width(0), height(0) {}
 741 template<typename _Tp> inline Rect_<_Tp>::Rect_(_Tp _x, _Tp _y, _Tp _width, _Tp _height) : x(_x), y(_y), width(_width), height(_height) {}
 742 template<typename _Tp> inline Rect_<_Tp>::Rect_(const Rect_<_Tp>& r) : x(r.x), y(r.y), width(r.width), height(r.height) {}
 743 template<typename _Tp> inline Rect_<_Tp>::Rect_(const CvRect& r) : x((_Tp)r.x), y((_Tp)r.y), width((_Tp)r.width), height((_Tp)r.height) {}
 744 template<typename _Tp> inline Rect_<_Tp>::Rect_(const Point_<_Tp>& org, const Size_<_Tp>& sz) :
 745     x(org.x), y(org.y), width(sz.width), height(sz.height) {}
 746 template<typename _Tp> inline Rect_<_Tp>::Rect_(const Point_<_Tp>& pt1, const Point_<_Tp>& pt2)
 747 {
 748     x = std::min(pt1.x, pt2.x); y = std::min(pt1.y, pt2.y);
 749     width = std::max(pt1.x, pt2.x) - x; height = std::max(pt1.y, pt2.y) - y;
 750 }
 751 template<typename _Tp> inline Rect_<_Tp>& Rect_<_Tp>::operator = ( const Rect_<_Tp>& r )
 752 { x = r.x; y = r.y; width = r.width; height = r.height; return *this; }
 753
 754 template<typename _Tp> inline Point_<_Tp> Rect_<_Tp>::tl() const { return Point_<_Tp>(x,y); }
 755 template<typename _Tp> inline Point_<_Tp> Rect_<_Tp>::br() const { return Point_<_Tp>(x+width, y+height); }
 756
 757 template<typename _Tp> static inline Rect_<_Tp>& operator += ( Rect_<_Tp>& a, const Point_<_Tp>& b )
 758 { a.x += b.x; a.y += b.y; return a; }
 759 template<typename _Tp> static inline Rect_<_Tp>& operator -= ( Rect_<_Tp>& a, const Point_<_Tp>& b )
 760 { a.x -= b.x; a.y -= b.y; return a; }
 761
 762 template<typename _Tp> static inline Rect_<_Tp>& operator += ( Rect_<_Tp>& a, const Size_<_Tp>& b )
 763 { a.width += b.width; a.height += b.height; return a; }
 764
 765 template<typename _Tp> static inline Rect_<_Tp>& operator -= ( Rect_<_Tp>& a, const Size_<_Tp>& b )
 766 { a.width -= b.width; a.height -= b.height; return a; }
 767
 768 template<typename _Tp> static inline Rect_<_Tp>& operator &= ( Rect_<_Tp>& a, const Rect_<_Tp>& b )
 769 {
 770     _Tp x1 = std::max(a.x, b.x), y1 = std::max(a.y, b.y);
 771     a.width = std::min(a.x + a.width, b.x + b.width) - x1;
 772     a.height = std::min(a.y + a.height, b.y + b.height) - y1;
 773     a.x = x1; a.y = y1;
 774     if( a.width <= 0 || a.height <= 0 )
 775         a = Rect();
 776     return a;
 777 }
 778
 779 template<typename _Tp> static inline Rect_<_Tp>& operator |= ( Rect_<_Tp>& a, const Rect_<_Tp>& b )
 780 {
 781     _Tp x1 = std::min(a.x, b.x), y1 = std::min(a.y, b.y);
 782     a.width = std::max(a.x + a.width, b.x + b.width) - x1;
 783     a.height = std::max(a.y + a.height, b.y + b.height) - y1;
 784     a.x = x1; a.y = y1;
 785     return a;
 786 }
 787
 788 template<typename _Tp> inline Size_<_Tp> Rect_<_Tp>::size() const { return Size_<_Tp>(width, height); }
 789 template<typename _Tp> inline _Tp Rect_<_Tp>::area() const { return width*height; }
 790
 791 template<typename _Tp> template<typename _Tp2> inline Rect_<_Tp>::operator Rect_<_Tp2>() const
 792 { return Rect_<_Tp2>(saturate_cast<_Tp2>(x), saturate_cast<_Tp2>(y),
 793                      saturate_cast<_Tp2>(width), saturate_cast<_Tp2>(height)); }
 794 template<typename _Tp> inline Rect_<_Tp>::operator CvRect() const
 795 { return cvRect(saturate_cast<int>(x), saturate_cast<int>(y),
 796                 saturate_cast<int>(width), saturate_cast<int>(height)); }
 797
 798 template<typename _Tp> inline bool Rect_<_Tp>::contains(const Point_<_Tp>& pt) const
 799 { return x <= pt.x && pt.x < x + width && y <= pt.y && pt.y < y + height; }
 800
 801 template<typename _Tp> static inline bool operator == (const Rect_<_Tp>& a, const Rect_<_Tp>& b)
 802 {
 803     return a.x == b.x && a.y == b.y && a.width == b.width && a.height == b.height;
 804 }
 805
 806 template<typename _Tp> static inline Rect_<_Tp> operator + (const Rect_<_Tp>& a, const Point_<_Tp>& b)
 807 {
 808     return Rect_<_Tp>( a.x + b.x, a.y + b.y, a.width, a.height );
 809 }
 810
 811 template<typename _Tp> static inline Rect_<_Tp> operator - (const Rect_<_Tp>& a, const Point_<_Tp>& b)
 812 {
 813     return Rect_<_Tp>( a.x - b.x, a.y - b.y, a.width, a.height );
 814 }
 815
 816 template<typename _Tp> static inline Rect_<_Tp> operator + (const Rect_<_Tp>& a, const Size_<_Tp>& b)
 817 {
 818     return Rect_<_Tp>( a.x, a.y, a.width + b.width, a.height + b.height );
 819 }
 820
 821 template<typename _Tp> static inline Rect_<_Tp> operator & (const Rect_<_Tp>& a, const Rect_<_Tp>& b)
 822 {
 823     Rect_<_Tp> c = a;
 824     return c &= b;
 825 }
 826
 827 template<typename _Tp> static inline Rect_<_Tp> operator | (const Rect_<_Tp>& a, const Rect_<_Tp>& b)
 828 {
 829     Rect_<_Tp> c = a;
 830     return c |= b;
 831 }
 832
 833 template<typename _Tp> inline bool Point_<_Tp>::inside( const Rect_<_Tp>& r ) const
 834 {
 835     return r.contains(*this);
 836 }
 837
 838 inline RotatedRect::RotatedRect() { angle = 0; }
 839 inline RotatedRect::RotatedRect(const Point2f& _center, const Size2f& _size, float _angle)
 840     : center(_center), size(_size), angle(_angle) {}
 841 inline RotatedRect::RotatedRect(const CvBox2D& box)
 842     : center(box.center), size(box.size), angle(box.angle) {}
 843 inline RotatedRect::operator CvBox2D() const
 844 {
 845     CvBox2D box; box.center = center; box.size = size; box.angle = angle;
 846     return box;
 847 }
 848 inline void RotatedRect::points(Point2f pt[]) const
 849 {
 850     double _angle = angle*CV_PI/180.;
 851     float a = (float)cos(_angle)*0.5f;
 852     float b = (float)sin(_angle)*0.5f;
 853
 854     pt[0].x = center.x - a*size.height - b*size.width;
 855     pt[0].y = center.y + b*size.height - a*size.width;
 856     pt[1].x = center.x + a*size.height - b*size.width;
 857     pt[1].y = center.y - b*size.height - a*size.width;
 858     pt[2].x = 2*center.x - pt[0].x;
 859     pt[2].y = 2*center.y - pt[0].y;
 860     pt[3].x = 2*center.x - pt[1].x;
 861     pt[3].y = 2*center.y - pt[1].y;
 862 }
 863
 864 inline Rect RotatedRect::boundingRect() const
 865 {
 866     Point2f pt[4];
 867     points(pt);
 868     Rect r(cvFloor(min(min(min(pt[0].x, pt[1].x), pt[2].x), pt[3].x)),
 869            cvFloor(min(min(min(pt[0].y, pt[1].y), pt[2].y), pt[3].y)),
 870            cvCeil(max(max(max(pt[0].x, pt[1].x), pt[2].x), pt[3].x)),
 871            cvCeil(max(max(max(pt[0].y, pt[1].y), pt[2].y), pt[3].y)));
 872     r.width -= r.x - 1;
 873     r.height -= r.y - 1;
 874     return r;
 875 }
 876
 877 //////////////////////////////// Scalar_ ///////////////////////////////
 878
 879 template<typename _Tp> inline Scalar_<_Tp>::Scalar_()
 880 { this->val[0] = this->val[1] = this->val[2] = this->val[3] = 0; }
 881
 882 template<typename _Tp> inline Scalar_<_Tp>::Scalar_(_Tp v0, _Tp v1, _Tp v2, _Tp v3)
 883 { this->val[0] = v0; this->val[1] = v1; this->val[2] = v2; this->val[3] = v3; }
 884
 885 template<typename _Tp> inline Scalar_<_Tp>::Scalar_(const CvScalar& s)
 886 {
 887     this->val[0] = saturate_cast<_Tp>(s.val[0]);
 888     this->val[1] = saturate_cast<_Tp>(s.val[1]);
 889     this->val[2] = saturate_cast<_Tp>(s.val[2]);
 890     this->val[3] = saturate_cast<_Tp>(s.val[3]);
 891 }
 892
 893 template<typename _Tp> inline Scalar_<_Tp>::Scalar_(_Tp v0)
 894 { this->val[0] = v0; this->val[1] = this->val[2] = this->val[3] = 0; }
 895
 896 template<typename _Tp> inline Scalar_<_Tp> Scalar_<_Tp>::all(_Tp v0)
 897 { return Scalar_<_Tp>(v0, v0, v0, v0); }
 898 template<typename _Tp> inline Scalar_<_Tp>::operator CvScalar() const
 899 { return cvScalar(this->val[0], this->val[1], this->val[2], this->val[3]); }
 900
 901 template<typename _Tp> template<typename T2> inline Scalar_<_Tp>::operator Scalar_<T2>() const
 902 {
 903     return Scalar_<T2>(saturate_cast<T2>(this->val[0]),
 904                   saturate_cast<T2>(this->val[1]),
 905                   saturate_cast<T2>(this->val[2]),
 906                   saturate_cast<T2>(this->val[3]));
 907 }
 908
 909 template<typename _Tp> static inline Scalar_<_Tp>& operator += (Scalar_<_Tp>& a, const Scalar_<_Tp>& b)
 910 {
 911     a.val[0] = saturate_cast<_Tp>(a.val[0] + b.val[0]);
 912     a.val[1] = saturate_cast<_Tp>(a.val[1] + b.val[1]);
 913     a.val[2] = saturate_cast<_Tp>(a.val[2] + b.val[2]);
 914     a.val[3] = saturate_cast<_Tp>(a.val[3] + b.val[3]);
 915     return a;
 916 }
 917
 918 template<typename _Tp> static inline Scalar_<_Tp>& operator -= (Scalar_<_Tp>& a, const Scalar_<_Tp>& b)
 919 {
 920     a.val[0] = saturate_cast<_Tp>(a.val[0] - b.val[0]);
 921     a.val[1] = saturate_cast<_Tp>(a.val[1] - b.val[1]);
 922     a.val[2] = saturate_cast<_Tp>(a.val[2] - b.val[2]);
 923     a.val[3] = saturate_cast<_Tp>(a.val[3] - b.val[3]);
 924     return a;
 925 }
 926
 927 template<typename _Tp> static inline Scalar_<_Tp>& operator *= ( Scalar_<_Tp>& a, _Tp v )
 928 {
 929     a.val[0] = saturate_cast<_Tp>(a.val[0] * v);
 930     a.val[1] = saturate_cast<_Tp>(a.val[1] * v);
 931     a.val[2] = saturate_cast<_Tp>(a.val[2] * v);
 932     a.val[3] = saturate_cast<_Tp>(a.val[3] * v);
 933     return a;
 934 }
 935
 936 template<typename _Tp> inline Scalar_<_Tp> Scalar_<_Tp>::mul(const Scalar_<_Tp>& t, double scale ) const
 937 {
 938     return Scalar_<_Tp>( saturate_cast<_Tp>(this->val[0]*t.val[0]*scale),
 939                        saturate_cast<_Tp>(this->val[1]*t.val[1]*scale),
 940                        saturate_cast<_Tp>(this->val[2]*t.val[2]*scale),
 941                        saturate_cast<_Tp>(this->val[3]*t.val[3]*scale));
 942 }
 943
 944 template<typename _Tp> static inline bool operator == ( const Scalar_<_Tp>& a, const Scalar_<_Tp>& b )
 945 {
 946     return a.val[0] == b.val[0] && a.val[1] == b.val[1] &&
 947         a.val[2] == b.val[2] && a.val[3] == b.val[3];
 948 }
 949
 950 template<typename _Tp> static inline bool operator != ( const Scalar_<_Tp>& a, const Scalar_<_Tp>& b )
 951 {
 952     return a.val[0] != b.val[0] || a.val[1] != b.val[1] ||
 953         a.val[2] != b.val[2] || a.val[3] != b.val[3];
 954 }
 955
 956 template<typename _Tp> template<typename T2> inline void Scalar_<_Tp>::convertTo(T2* buf, int cn, int unroll_to) const
 957 {
 958     int i;
 959     CV_Assert(cn <= 4);
 960     for( i = 0; i < cn; i++ )
 961         buf[i] = saturate_cast<T2>(this->val[i]);
 962     for( ; i < unroll_to; i++ )
 963         buf[i] = buf[i-cn];
 964 }
 965
 966 static inline void scalarToRawData(const Scalar& s, void* buf, int type, int unroll_to=0)
 967 {
 968     int depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type);
 969     switch(depth)
 970     {
 971     case CV_8U:
 972         s.convertTo((uchar*)buf, cn, unroll_to);
 973         break;
 974     case CV_8S:
 975         s.convertTo((schar*)buf, cn, unroll_to);
 976         break;
 977     case CV_16U:
 978         s.convertTo((ushort*)buf, cn, unroll_to);
 979         break;
 980     case CV_16S:
 981         s.convertTo((short*)buf, cn, unroll_to);
 982         break;
 983     case CV_32S:
 984         s.convertTo((int*)buf, cn, unroll_to);
 985         break;
 986     case CV_32F:
 987         s.convertTo((float*)buf, cn, unroll_to);
 988         break;
 989     case CV_64F:
 990         s.convertTo((double*)buf, cn, unroll_to);
 991         break;
 992     default:
 993         CV_Error(CV_StsUnsupportedFormat,"");
 994     }
 995 }
 996
 997
 998 template<typename _Tp> static inline Scalar_<_Tp> operator + (const Scalar_<_Tp>& a, const Scalar_<_Tp>& b)
 999 {
1000     return Scalar_<_Tp>(saturate_cast<_Tp>(a.val[0] + b.val[0]),
1001                       saturate_cast<_Tp>(a.val[1] + b.val[1]),
1002                       saturate_cast<_Tp>(a.val[2] + b.val[2]),
1003                       saturate_cast<_Tp>(a.val[3] + b.val[3]));
1004 }
1005
1006 template<typename _Tp> static inline Scalar_<_Tp> operator - (const Scalar_<_Tp>& a, const Scalar_<_Tp>& b)
1007 {
1008     return Scalar_<_Tp>(saturate_cast<_Tp>(a.val[0] - b.val[0]),
1009                       saturate_cast<_Tp>(a.val[1] - b.val[1]),
1010                       saturate_cast<_Tp>(a.val[2] - b.val[2]),
1011                       saturate_cast<_Tp>(a.val[3] - b.val[3]));
1012 }
1013
1014 template<typename _Tp> static inline Scalar_<_Tp> operator * (const Scalar_<_Tp>& a, _Tp alpha)
1015 {
1016     return Scalar_<_Tp>(saturate_cast<_Tp>(a.val[0] * alpha),
1017                       saturate_cast<_Tp>(a.val[1] * alpha),
1018                       saturate_cast<_Tp>(a.val[2] * alpha),
1019                       saturate_cast<_Tp>(a.val[3] * alpha));
1020 }
1021
1022 template<typename _Tp> static inline Scalar_<_Tp> operator * (_Tp alpha, const Scalar_<_Tp>& a)
1023 {
1024     return a*alpha;
1025 }
1026
1027 template<typename _Tp> static inline Scalar_<_Tp> operator - (const Scalar_<_Tp>& a)
1028 {
1029     return Scalar_<_Tp>(saturate_cast<_Tp>(-a.val[0]), saturate_cast<_Tp>(-a.val[1]),
1030                       saturate_cast<_Tp>(-a.val[2]), saturate_cast<_Tp>(-a.val[3]));
1031 }
1032
1033 //////////////////////////////// Range /////////////////////////////////
1034
1035 inline Range::Range() : start(0), end(0) {}
1036 inline Range::Range(int _start, int _end) : start(_start), end(_end) {}
1037 inline Range::Range(const CvSlice& slice) : start(slice.start_index), end(slice.end_index)
1038 {
1039     if( start == 0 && end == CV_WHOLE_SEQ_END_INDEX )
1040         *this = Range::all();
1041 }
1042
1043 inline int Range::size() const { return end - start; }
1044 inline bool Range::empty() const { return start == end; }
1045 inline Range Range::all() { return Range(INT_MIN, INT_MAX); }
1046
1047 static inline bool operator == (const Range& r1, const Range& r2)
1048 { return r1.start == r2.start && r1.end == r2.end; }
1049
1050 static inline bool operator != (const Range& r1, const Range& r2)
1051 { return !(r1 == r2); }
1052
1053 static inline bool operator !(const Range& r)
1054 { return r.start == r.end; }
1055
1056 static inline Range operator & (const Range& r1, const Range& r2)
1057 {
1058     Range r(std::max(r1.start, r2.start), std::min(r2.start, r2.end));
1059     r.end = std::max(r.end, r.start);
1060     return r;
1061 }
1062
1063 static inline Range& operator &= (Range& r1, const Range& r2)
1064 {
1065     r1 = r1 & r2;
1066     return r1;
1067 }
1068
1069 static inline Range operator + (const Range& r1, int delta)
1070 {
1071     return Range(r1.start + delta, r1.end + delta);
1072 }
1073
1074 static inline Range operator + (int delta, const Range& r1)
1075 {
1076     return Range(r1.start + delta, r1.end + delta);
1077 }
1078
1079 static inline Range operator - (const Range& r1, int delta)
1080 {
1081     return r1 + (-delta);
1082 }
1083
1084 inline Range::operator CvSlice() const
1085 { return *this != Range::all() ? cvSlice(start, end) : CV_WHOLE_SEQ; }
1086
1087
1088
1089 //////////////////////////////// Vector ////////////////////////////////
1090
1091 // template vector class. It is similar to STL's vector,
1092 // with a few important differences:
1093 //   1) it can be created on top of user-allocated data w/o copying it
1094 //   2) vector b = a means copying the header,
1095 //      not the underlying data (use clone() to make a deep copy)
1096 template <typename _Tp> class CV_EXPORTS Vector
1097 {
1098 public:
1099     typedef _Tp value_type;
1100     typedef _Tp* iterator;
1101     typedef const _Tp* const_iterator;
1102     typedef _Tp& reference;
1103     typedef const _Tp& const_reference;
1104
1105     struct CV_EXPORTS Hdr
1106     {
1107         Hdr() : data(0), datastart(0), refcount(0), size(0), capacity(0) {};
1108         _Tp* data;
1109         _Tp* datastart;
1110         int* refcount;
1111         size_t size;
1112         size_t capacity;
1113     };
1114
1115     Vector() {}
1116     Vector(size_t _size)  { resize(_size); }
1117     Vector(size_t _size, const _Tp& val)
1118     {
1119         resize(_size);
1120         for(size_t i = 0; i < _size; i++)
1121             hdr.data[i] = val;
1122     }
1123     Vector(_Tp* _data, size_t _size, bool _copyData=false)
1124     { set(_data, _size, _copyData); }
1125
1126     template<int n> Vector(const Vec<_Tp, n>& vec)
1127     { set((_Tp*)&vec.val[0], n, true); }
1128
1129     Vector(const std::vector<_Tp>& vec, bool _copyData=false)
1130     { set((_Tp*)&vec[0], vec.size(), _copyData); }
1131
1132     Vector(const Vector& d) { *this = d; }
1133
1134     Vector(const Vector& d, const Range& r)
1135     {
1136         if( r == Range::all() )
1137             r = Range(0, d.size());
1138         if( r.size() > 0 && r.start >= 0 && r.end <= d.size() )
1139         {
1140             if( d.hdr.refcount )
1141                 CV_XADD(d.hdr.refcount, 1);
1142             hdr.refcount = d.hdr.refcount;
1143             hdr.datastart = d.hdr.datastart;
1144             hdr.data = d.hdr.data + r.start;
1145             hdr.capacity = hdr.size = r.size();
1146         }
1147     }
1148
1149     Vector<_Tp>& operator = (const Vector& d)
1150     {
1151         if( this != &d )
1152         {
1153             if( d.hdr.refcount )
1154                 CV_XADD(d.hdr.refcount, 1);
1155             release();
1156             hdr = d.hdr;
1157         }
1158         return *this;
1159     }
1160
1161     ~Vector()  { release(); }
1162
1163     Vector<_Tp> clone() const
1164     { return hdr.data ? Vector<_Tp>(hdr.data, hdr.size, true) : Vector<_Tp>(); }
1165
1166     void copyTo(Vector<_Tp>& vec) const
1167     {
1168         size_t i, sz = size();
1169         vec.resize(sz);
1170         const _Tp* src = hdr.data;
1171         _Tp* dst = vec.hdr.data;
1172         for( i = 0; i < sz; i++ )
1173             dst[i] = src[i];
1174     }
1175
1176     void copyTo(std::vector<_Tp>& vec) const
1177     {
1178         size_t i, sz = size();
1179         vec.resize(sz);
1180         const _Tp* src = hdr.data;
1181         _Tp* dst = sz ? &vec[0] : 0;
1182         for( i = 0; i < sz; i++ )
1183             dst[i] = src[i];
1184     }
1185
1186     operator CvMat() const
1187     { return cvMat((int)size(), 1, type(), (void*)hdr.data); }
1188
1189     _Tp& operator [] (size_t i) { CV_DbgAssert( i < size() ); return hdr.data[i]; }
1190     const _Tp& operator [] (size_t i) const { CV_DbgAssert( i < size() ); return hdr.data[i]; }
1191     Vector operator() (const Range& r) const { return Vector(*this, r); }
1192     _Tp& back() { CV_DbgAssert(!empty()); return hdr.data[hdr.size-1]; }
1193     const _Tp& back() const { CV_DbgAssert(!empty()); return hdr.data[hdr.size-1]; }
1194     _Tp& front() { CV_DbgAssert(!empty()); return hdr.data[0]; }
1195     const _Tp& front() const { CV_DbgAssert(!empty()); return hdr.data[0]; }
1196
1197     _Tp* begin() { return hdr.data; }
1198     _Tp* end() { return hdr.data + hdr.size; }
1199     const _Tp* begin() const { return hdr.data; }
1200     const _Tp* end() const { return hdr.data + hdr.size; }
1201
1202     void addref() { if( hdr.refcount ) CV_XADD(hdr.refcount, 1); }
1203     void release()
1204     {
1205         if( hdr.refcount && CV_XADD(hdr.refcount, -1) == 1 )
1206             deallocate<_Tp>(hdr.datastart, hdr.capacity);
1207         hdr = Hdr();
1208     }
1209
1210     void set(_Tp* _data, size_t _size, bool _copyData=false)
1211     {
1212         if( !_copyData )
1213         {
1214             release();
1215             hdr.data = hdr.datastart = _data;
1216             hdr.size = hdr.capacity = _size;
1217             hdr.refcount = 0;
1218         }
1219         else
1220         {
1221             reserve(_size);
1222             for( size_t i = 0; i < _size; i++ )
1223                 hdr.data[i] = _data[i];
1224             hdr.size = _size;
1225         }
1226     }
1227
1228     void reserve(size_t newCapacity)
1229     {
1230         _Tp* newData;
1231         int* newRefcount;
1232         size_t i, oldSize = hdr.size;
1233         if( (!hdr.refcount || *hdr.refcount == 1) && hdr.capacity >= newCapacity )
1234             return;
1235         newCapacity = std::max(newCapacity, oldSize);
1236         size_t datasize = alignSize(newCapacity*sizeof(_Tp), (size_t)sizeof(*newRefcount));
1237         newData = (_Tp*)fastMalloc(datasize + sizeof(*newRefcount));
1238         for( i = 0; i < oldSize; i++ )
1239             ::new(newData + i) _Tp(hdr.data[i]);
1240         _Tp dummy = _Tp();
1241         for( ; i < newCapacity; i++ )
1242             ::new(newData + i) _Tp(dummy);
1243         newRefcount = (int*)((uchar*)newData + datasize);
1244         *newRefcount = 1;
1245         release();
1246         hdr.data = hdr.datastart = newData;
1247         hdr.capacity = newCapacity;
1248         hdr.size = oldSize;
1249         hdr.refcount = newRefcount;
1250     }
1251
1252     void resize(size_t newSize)
1253     {
1254         size_t i;
1255         newSize = std::max(newSize, (size_t)0);
1256         if( (!hdr.refcount || *hdr.refcount == 1) && hdr.size == newSize )
1257             return;
1258         if( newSize > hdr.capacity )
1259             reserve(std::max(newSize, std::max((size_t)4, hdr.capacity*2)));
1260         for( i = hdr.size; i < newSize; i++ )
1261             hdr.data[i] = _Tp();
1262         hdr.size = newSize;
1263     }
1264
1265     Vector<_Tp>& push_back(const _Tp& elem)
1266     {
1267         if( hdr.size == hdr.capacity )
1268             reserve( std::max((size_t)4, hdr.capacity*2) );
1269         hdr.data[hdr.size++] = elem;
1270         return *this;
1271     }
1272
1273     Vector<_Tp>& pop_back()
1274     {
1275         if( hdr.size > 0 )
1276             --hdr.size;
1277         return *this;
1278     }
1279
1280     size_t size() const { return hdr.size; }
1281     size_t capacity() const { return hdr.capacity; }
1282     bool empty() const { return hdr.size == 0; }
1283     void clear() { resize(0); }
1284     int type() const { return DataType<_Tp>::type; }
1285
1286 protected:
1287     Hdr hdr;
1288 };
1289
1290
1291 template<typename _Tp> inline typename DataType<_Tp>::work_type
1292 dot(const Vector<_Tp>& v1, const Vector<_Tp>& v2)
1293 {
1294     typedef typename DataType<_Tp>::work_type _Tw;
1295     size_t i, n = v1.size();
1296     assert(v1.size() == v2.size());
1297
1298     _Tw s = 0;
1299     const _Tp *ptr1 = &v1[0], *ptr2 = &v2[0];
1300     for( i = 0; i <= n - 4; i += 4 )
1301         s += (_Tw)ptr1[i]*ptr2[i] + (_Tw)ptr1[i+1]*ptr2[i+1] +
1302             (_Tw)ptr1[i+2]*ptr2[i+2] + (_Tw)ptr1[i+3]*ptr2[i+3];
1303     for( ; i < n; i++ )
1304         s += (_Tw)ptr1[i]*ptr2[i];
1305     return s;
1306 }
1307
1308 // Multiply-with-Carry RNG
1309 inline RNG::RNG() { state = 0xffffffff; }
1310 inline RNG::RNG(uint64 _state) { state = _state ? _state : 0xffffffff; }
1311 inline unsigned RNG::next()
1312 {
1313     state = (uint64)(unsigned)state*A + (unsigned)(state >> 32);
1314     return (unsigned)state;
1315 }
1316
1317 inline RNG::operator uchar() { return (uchar)next(); }
1318 inline RNG::operator schar() { return (schar)next(); }
1319 inline RNG::operator ushort() { return (ushort)next(); }
1320 inline RNG::operator short() { return (short)next(); }
1321 inline RNG::operator unsigned() { return next(); }
1322 inline RNG::operator int() { return (int)next(); }
1323 // * (2^32-1)^-1
1324 inline RNG::operator float() { return next()*2.3283064365386962890625e-10f; }
1325 inline RNG::operator double()
1326 {
1327     unsigned t = next();
1328     return (((uint64)t << 32) | next())*5.4210108624275221700372640043497e-20;
1329 }
1330 inline int RNG::uniform(int a, int b) { return a == b ? a : next()%(b - a) + a; }
1331 inline float RNG::uniform(float a, float b) { return ((float)*this)*(b - a) + a; }
1332 inline double RNG::uniform(double a, double b) { return ((float)*this)*(b - a) + a; }
1333
1334
1335 inline TermCriteria::TermCriteria() : type(0), maxCount(0), epsilon(0) {}
1336 inline TermCriteria::TermCriteria(int _type, int _maxCount, double _epsilon)
1337     : type(_type), maxCount(_maxCount), epsilon(_epsilon) {}
1338 inline TermCriteria::TermCriteria(const CvTermCriteria& criteria)
1339     : type(criteria.type), maxCount(criteria.max_iter), epsilon(criteria.epsilon) {}
1340 inline TermCriteria::operator CvTermCriteria() const
1341 { return cvTermCriteria(type, maxCount, epsilon); }
1342
1343 inline uchar* LineIterator::operator *() { return ptr; }
1344 inline LineIterator& LineIterator::operator ++()
1345 {
1346     int mask = err < 0 ? -1 : 0;
1347     err += minusDelta + (plusDelta & mask);
1348     ptr += minusStep + (plusStep & mask);
1349     return *this;
1350 }
1351 inline LineIterator LineIterator::operator ++(int)
1352 {
1353     LineIterator it = *this;
1354     ++(*this);
1355     return it;
1356 }
1357
1358 #if 0
1359   template<typename _Tp> inline VectorCommaInitializer_<_Tp>::
1360   VectorCommaInitializer_(vector<_Tp>* _vec) : vec(_vec), idx(0) {}
1361
1362   template<typename _Tp> template<typename T2> inline VectorCommaInitializer_<_Tp>&
1363   VectorCommaInitializer_<_Tp>::operator , (T2 val)
1364   {
1365       if( (size_t)idx < vec->size() )
1366           (*vec)[idx] = _Tp(val);
1367       else
1368           vec->push_back(_Tp(val));
1369       idx++;
1370       return *this;
1371   }
1372
1373   template<typename _Tp> inline VectorCommaInitializer_<_Tp>::operator vector<_Tp>() const
1374   { return *vec; }
1375
1376   template<typename _Tp> inline vector<_Tp> VectorCommaInitializer_<_Tp>::operator *() const
1377   { return *vec; }
1378
1379   template<typename _Tp, typename T2> static inline VectorCommaInitializer_<_Tp>
1380   operator << (const vector<_Tp>& vec, T2 val)
1381   {
1382       VectorCommaInitializer_<_Tp> commaInitializer((vector<_Tp>*)&vec);
1383       return (commaInitializer, val);
1384   }
1385 #endif
1386
1387 /////////////////////////////// AutoBuffer ////////////////////////////////////////
1388
1389 template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>::AutoBuffer()
1390 : ptr(buf), size(fixed_size) {}
1391
1392 template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>::AutoBuffer(size_t _size)
1393 : ptr(buf), size(fixed_size) { allocate(_size); }
1394
1395 template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>::~AutoBuffer()
1396 { deallocate(); }
1397
1398 template<typename _Tp, size_t fixed_size> inline void AutoBuffer<_Tp, fixed_size>::allocate(size_t _size)
1399 {
1400     if(_size <= size)
1401         return;
1402     deallocate();
1403     if(_size > fixed_size)
1404     {
1405         ptr = cv::allocate<_Tp>(_size);
1406         size = _size;
1407     }
1408 }
1409
1410 template<typename _Tp, size_t fixed_size> inline void AutoBuffer<_Tp, fixed_size>::deallocate()
1411 {
1412     if( ptr != buf )
1413     {
1414         cv::deallocate<_Tp>(ptr, size);
1415         ptr = buf;
1416         size = fixed_size;
1417     }
1418 }
1419
1420 template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>::operator _Tp* ()
1421 { return ptr; }
1422
1423 template<typename _Tp, size_t fixed_size> inline AutoBuffer<_Tp, fixed_size>::operator const _Tp* () const
1424 { return ptr; }
1425
1426
1427 /////////////////////////////////// Ptr ////////////////////////////////////////
1428
1429 template<typename _Tp> inline Ptr<_Tp>::Ptr() : obj(0), refcount(0) {}
1430 template<typename _Tp> inline Ptr<_Tp>::Ptr(_Tp* _obj) : obj(_obj)
1431 {
1432     if(obj)
1433     {
1434         refcount = (int*)fastMalloc(sizeof(*refcount));
1435         *refcount = 1;
1436     }
1437     else
1438         refcount = 0;
1439 }
1440
1441 template<typename _Tp> inline void Ptr<_Tp>::addref()
1442 { if( refcount ) CV_XADD(refcount, 1); }
1443
1444 template<typename _Tp> inline void Ptr<_Tp>::release()
1445 {
1446     if( refcount && CV_XADD(refcount, -1) == 1 )
1447     {
1448         delete_obj();
1449         fastFree(refcount);
1450     }
1451     refcount = 0;
1452     obj = 0;
1453 }
1454
1455 template<typename _Tp> inline void Ptr<_Tp>::delete_obj()
1456 {
1457     if( obj ) delete obj;
1458 }
1459
1460 template<typename _Tp> inline Ptr<_Tp>::~Ptr() { release(); }
1461
1462 template<typename _Tp> inline Ptr<_Tp>::Ptr(const Ptr<_Tp>& ptr)
1463 {
1464     obj = ptr.obj;
1465     refcount = ptr.refcount;
1466     addref();
1467 }
1468
1469 template<typename _Tp> inline Ptr<_Tp>& Ptr<_Tp>::operator = (const Ptr<_Tp>& ptr)
1470 {
1471     int* _refcount = ptr.refcount;
1472     if( _refcount )
1473         CV_XADD(_refcount, 1);
1474     release();
1475     obj = ptr.obj;
1476     refcount = _refcount;
1477     return *this;
1478 }
1479
1480 template<typename _Tp> inline _Tp* Ptr<_Tp>::operator -> () { return obj; }
1481 template<typename _Tp> inline const _Tp* Ptr<_Tp>::operator -> () const { return obj; }
1482
1483 template<typename _Tp> inline Ptr<_Tp>::operator _Tp* () { return obj; }
1484 template<typename _Tp> inline Ptr<_Tp>::operator const _Tp*() const { return obj; }
1485
1486 template<typename _Tp> inline bool Ptr<_Tp>::empty() const { return obj == 0; }
1487
1488 //// specializied implementations of Ptr::delete_obj() for classic OpenCV types
1489
1490 template<> inline void Ptr<CvMat>::delete_obj()
1491 { cvReleaseMat(&obj); }
1492
1493 template<> inline void Ptr<IplImage>::delete_obj()
1494 { cvReleaseImage(&obj); }
1495
1496 template<> inline void Ptr<CvMatND>::delete_obj()
1497 { cvReleaseMatND(&obj); }
1498
1499 template<> inline void Ptr<CvSparseMat>::delete_obj()
1500 { cvReleaseSparseMat(&obj); }
1501
1502 template<> inline void Ptr<CvMemStorage>::delete_obj()
1503 { cvReleaseMemStorage(&obj); }
1504
1505 template<> inline void Ptr<CvFileStorage>::delete_obj()
1506 { cvReleaseFileStorage(&obj); }
1507
1508 //////////////////////////////////////// XML & YAML I/O ////////////////////////////////////
1509
1510 static inline void write( FileStorage& fs, const string& name, int value )
1511 { cvWriteInt( *fs, name.size() ? name.c_str() : 0, value ); }
1512
1513 static inline void write( FileStorage& fs, const string& name, float value )
1514 { cvWriteReal( *fs, name.size() ? name.c_str() : 0, value ); }
1515
1516 static inline void write( FileStorage& fs, const string& name, double value )
1517 { cvWriteReal( *fs, name.size() ? name.c_str() : 0, value ); }
1518
1519 static inline void write( FileStorage& fs, const string& name, const string& value )
1520 { cvWriteString( *fs, name.size() ? name.c_str() : 0, value.c_str() ); }
1521
1522 template<typename _Tp> static inline void write(FileStorage& fs, const _Tp& value)
1523 { write(fs, string(), value); }
1524
1525 template<> inline void write(FileStorage& fs, const int& value )
1526 { cvWriteInt( *fs, 0, value ); }
1527
1528 template<> inline void write(FileStorage& fs, const float& value )
1529 { cvWriteReal( *fs, 0, value ); }
1530
1531 template<> inline void write(FileStorage& fs, const double& value )
1532 { cvWriteReal( *fs, 0, value ); }
1533
1534 template<> inline void write(FileStorage& fs, const string& value )
1535 { cvWriteString( *fs, 0, value.c_str() ); }
1536
1537 template<typename _Tp> inline void write(FileStorage& fs, const Point_<_Tp>& pt )
1538 {
1539     write(fs, pt.x);
1540     write(fs, pt.y);
1541 }
1542
1543 template<typename _Tp> inline void write(FileStorage& fs, const Point3_<_Tp>& pt )
1544 {
1545     write(fs, pt.x);
1546     write(fs, pt.y);
1547     write(fs, pt.z);
1548 }
1549
1550 template<typename _Tp> inline void write(FileStorage& fs, const Size_<_Tp>& sz )
1551 {
1552     write(fs, sz.width);
1553     write(fs, sz.height);
1554 }
1555
1556 template<typename _Tp> inline void write(FileStorage& fs, const Complex<_Tp>& c )
1557 {
1558     write(fs, c.re);
1559     write(fs, c.im);
1560 }
1561
1562 template<typename _Tp> inline void write(FileStorage& fs, const Rect_<_Tp>& r )
1563 {
1564     write(fs, r.x);
1565     write(fs, r.y);
1566     write(fs, r.width);
1567     write(fs, r.height);
1568 }
1569
1570 template<typename _Tp, int cn> inline void write(FileStorage& fs, const Vec<_Tp, cn>& v )
1571 {
1572     for(int i = 0; i < cn; i++)
1573         write(fs, v.val[i]);
1574 }
1575
1576 template<typename _Tp> inline void write(FileStorage& fs, const Scalar_<_Tp>& s )
1577 {
1578     write(fs, s.val[0]);
1579     write(fs, s.val[1]);
1580     write(fs, s.val[2]);
1581     write(fs, s.val[3]);
1582 }
1583
1584 inline void write(FileStorage& fs, const Range& r )
1585 {
1586     write(fs, r.start);
1587     write(fs, r.end);
1588 }
1589
1590 class CV_EXPORTS WriteStructContext
1591 {
1592 public:
1593     WriteStructContext(FileStorage& _fs, const string& name,
1594         int flags, const string& typeName=string()) : fs(&_fs)
1595     {
1596         cvStartWriteStruct(**fs, !name.empty() ? name.c_str() : 0, flags,
1597             !typeName.empty() ? typeName.c_str() : 0);
1598     }
1599     ~WriteStructContext() { cvEndWriteStruct(**fs); }
1600     FileStorage* fs;
1601 };
1602
1603 template<typename _Tp> inline void write(FileStorage& fs, const string& name, const Point_<_Tp>& pt )
1604 {
1605     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1606     write(fs, pt.x);
1607     write(fs, pt.y);
1608 }
1609
1610 template<typename _Tp> inline void write(FileStorage& fs, const string& name, const Point3_<_Tp>& pt )
1611 {
1612     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1613     write(fs, pt.x);
1614     write(fs, pt.y);
1615     write(fs, pt.z);
1616 }
1617
1618 template<typename _Tp> inline void write(FileStorage& fs, const string& name, const Size_<_Tp>& sz )
1619 {
1620     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1621     write(fs, sz.width);
1622     write(fs, sz.height);
1623 }
1624
1625 template<typename _Tp> inline void write(FileStorage& fs, const string& name, const Complex<_Tp>& c )
1626 {
1627     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1628     write(fs, c.re);
1629     write(fs, c.im);
1630 }
1631
1632 template<typename _Tp> inline void write(FileStorage& fs, const string& name, const Rect_<_Tp>& r )
1633 {
1634     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1635     write(fs, r.x);
1636     write(fs, r.y);
1637     write(fs, r.width);
1638     write(fs, r.height);
1639 }
1640
1641 template<typename _Tp, int cn> inline void write(FileStorage& fs, const string& name, const Vec<_Tp, cn>& v )
1642 {
1643     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1644     for(int i = 0; i < cn; i++)
1645         write(fs, v.val[i]);
1646 }
1647
1648 template<typename _Tp> inline void write(FileStorage& fs, const string& name, const Scalar_<_Tp>& s )
1649 {
1650     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1651     write(fs, s.val[0]);
1652     write(fs, s.val[1]);
1653     write(fs, s.val[2]);
1654     write(fs, s.val[3]);
1655 }
1656
1657 inline void write(FileStorage& fs, const string& name, const Range& r )
1658 {
1659     WriteStructContext ws(fs, name, CV_NODE_SEQ+CV_NODE_FLOW);
1660     write(fs, r.start);
1661     write(fs, r.end);
1662 }
1663
1664 template<typename _Tp, int numflag> class CV_EXPORTS VecWriterProxy
1665 {
1666 public:
1667     VecWriterProxy( FileStorage* _fs ) : fs(_fs) {}
1668     void operator()(const vector<_Tp>& vec) const
1669     {
1670         size_t i, count = vec.size();
1671         for( i = 0; i < count; i++ )
1672             write( *fs, vec[i] );
1673     }
1674     FileStorage* fs;
1675 };
1676
1677 template<typename _Tp> class CV_EXPORTS VecWriterProxy<_Tp,1>
1678 {
1679 public:
1680     VecWriterProxy( FileStorage* _fs ) : fs(_fs) {}
1681     void operator()(const vector<_Tp>& vec) const
1682     {
1683         int _fmt = DataType<_Tp>::fmt;
1684         char fmt[] = { (char)((_fmt>>8)+'1'), (char)_fmt, '\0' };
1685         fs->writeRaw( string(fmt), (uchar*)&vec[0], vec.size()*sizeof(_Tp) );
1686     }
1687     FileStorage* fs;
1688 };
1689
1690
1691 template<typename _Tp> static inline void write( FileStorage& fs, const vector<_Tp>& vec )
1692 {
1693     VecWriterProxy<_Tp, DataType<_Tp>::fmt != 0> w(&fs);
1694     w(vec);
1695 }
1696
1697 template<typename _Tp> static inline FileStorage&
1698 operator << ( FileStorage& fs, const vector<_Tp>& vec )
1699 {
1700     VecWriterProxy<_Tp, DataType<_Tp>::fmt != 0> w(&fs);
1701     w(vec);
1702     return fs;
1703 }
1704
1705 CV_EXPORTS void write( FileStorage& fs, const string& name, const Mat& value );
1706 CV_EXPORTS void write( FileStorage& fs, const string& name, const MatND& value );
1707 CV_EXPORTS void write( FileStorage& fs, const string& name, const SparseMat& value );
1708
1709 template<typename _Tp> static inline FileStorage& operator << (FileStorage& fs, const _Tp& value)
1710 {
1711     if( !fs.isOpened() )
1712         return fs;
1713     if( fs.state == FileStorage::NAME_EXPECTED + FileStorage::INSIDE_MAP )
1714         CV_Error( CV_StsError, "No element name has been given" );
1715     write( fs, fs.elname, value );
1716     if( fs.state & FileStorage::INSIDE_MAP )
1717         fs.state = FileStorage::NAME_EXPECTED + FileStorage::INSIDE_MAP;
1718     return fs;
1719 }
1720
1721 CV_EXPORTS FileStorage& operator << (FileStorage& fs, const string& str);
1722
1723 static inline FileStorage& operator << (FileStorage& fs, const char* str)
1724 { return (fs << string(str)); }
1725
1726 inline FileNode FileStorage::operator[](const string& nodename) const
1727 {
1728     return FileNode(fs, cvGetFileNodeByName(fs, 0, nodename.c_str()));
1729 }
1730 inline FileNode FileStorage::operator[](const char* nodename) const
1731 {
1732     return FileNode(fs, cvGetFileNodeByName(fs, 0, nodename));
1733 }
1734
1735 inline FileNode::FileNode() : fs(0), node(0) {}
1736 inline FileNode::FileNode(const CvFileStorage* _fs, const CvFileNode* _node)
1737     : fs(_fs), node(_node) {}
1738
1739 inline FileNode::FileNode(const FileNode& _node) : fs(_node.fs), node(_node.node) {}
1740 inline FileNode FileNode::operator[](const string& nodename) const
1741 {
1742     return FileNode(fs, cvGetFileNodeByName(fs, node, nodename.c_str()));
1743 }
1744 inline FileNode FileNode::operator[](const char* nodename) const
1745 {
1746     return FileNode(fs, cvGetFileNodeByName(fs, node, nodename));
1747 }
1748
1749 inline FileNode FileNode::operator[](int i) const
1750 {
1751     return isSeq() ? FileNode(fs, (CvFileNode*)cvGetSeqElem(node->data.seq, i)) :
1752         i == 0 ? *this : FileNode();
1753 }
1754
1755 inline int FileNode::type() const { return !node ? NONE : (node->tag & TYPE_MASK); }
1756 inline bool FileNode::empty() const { return node == 0; }
1757 inline bool FileNode::isNone() const { return type() == NONE; }
1758 inline bool FileNode::isSeq() const { return type() == SEQ; }
1759 inline bool FileNode::isMap() const { return type() == MAP; }
1760 inline bool FileNode::isInt() const { return type() == INT; }
1761 inline bool FileNode::isReal() const { return type() == REAL; }
1762 inline bool FileNode::isString() const { return type() == STR; }
1763 inline bool FileNode::isNamed() const { return !node ? false : (node->tag & NAMED) != 0; }
1764 inline string FileNode::name() const
1765 {
1766     const char* str;
1767     return !node || (str = cvGetFileNodeName(node)) == 0 ? string() : string(str);
1768 }
1769 inline size_t FileNode::size() const
1770 {
1771     int t = type();
1772     return t == MAP ? ((CvSet*)node->data.map)->active_count :
1773         t == SEQ ? node->data.seq->total : node != 0;
1774 }
1775
1776 inline CvFileNode* FileNode::operator *() { return (CvFileNode*)node; }
1777 inline const CvFileNode* FileNode::operator* () const { return node; }
1778
1779 static inline void read(const FileNode& node, bool& value, bool default_value)
1780 { value = cvReadInt(node.node, default_value) != 0; }
1781
1782 static inline void read(const FileNode& node, uchar& value, uchar default_value)
1783 { value = saturate_cast<uchar>(cvReadInt(node.node, default_value)); }
1784
1785 static inline void read(const FileNode& node, schar& value, schar default_value)
1786 { value = saturate_cast<schar>(cvReadInt(node.node, default_value)); }
1787
1788 static inline void read(const FileNode& node, ushort& value, ushort default_value)
1789 { value = saturate_cast<ushort>(cvReadInt(node.node, default_value)); }
1790
1791 static inline void read(const FileNode& node, short& value, short default_value)
1792 { value = saturate_cast<short>(cvReadInt(node.node, default_value)); }
1793
1794 static inline void read(const FileNode& node, int& value, int default_value)
1795 { value = cvReadInt(node.node, default_value); }
1796
1797 static inline void read(const FileNode& node, float& value, float default_value)
1798 { value = (float)cvReadReal(node.node, default_value); }
1799
1800 static inline void read(const FileNode& node, double& value, double default_value)
1801 { value = cvReadReal(node.node, default_value); }
1802
1803 static inline void read(const FileNode& node, string& value, const string& default_value)
1804 { value = string(cvReadString(node.node, default_value.c_str())); }
1805
1806 CV_EXPORTS void read(const FileNode& node, Mat& mat, const Mat& default_mat=Mat() );
1807 CV_EXPORTS void read(const FileNode& node, MatND& mat, const MatND& default_mat=MatND() );
1808 CV_EXPORTS void read(const FileNode& node, SparseMat& mat, const SparseMat& default_mat=SparseMat() );
1809
1810 inline FileNode::operator int() const
1811 {
1812     return cvReadInt(node, 0);
1813 }
1814 inline FileNode::operator float() const
1815 {
1816     return (float)cvReadReal(node, 0);
1817 }
1818 inline FileNode::operator double() const
1819 {
1820     return cvReadReal(node, 0);
1821 }
1822 inline FileNode::operator string() const
1823 {
1824     return string(cvReadString(node, ""));
1825 }
1826
1827 inline void FileNode::readRaw( const string& fmt, uchar* vec, size_t len ) const
1828 {
1829     begin().readRaw( fmt, vec, len );
1830 }
1831
1832 template<typename _Tp, int numflag> class CV_EXPORTS VecReaderProxy
1833 {
1834 public:
1835     VecReaderProxy( FileNodeIterator* _it ) : it(_it) {}
1836     void operator()(vector<_Tp>& vec, size_t count) const
1837     {
1838         count = std::min(count, it->remaining);
1839         vec.resize(count);
1840         for( size_t i = 0; i < count; i++, ++(*it) )
1841             read(**it, vec[i], _Tp());
1842     }
1843     FileNodeIterator* it;
1844 };
1845
1846 template<typename _Tp> class CV_EXPORTS VecReaderProxy<_Tp,1>
1847 {
1848 public:
1849     VecReaderProxy( FileNodeIterator* _it ) : it(_it) {}
1850     void operator()(vector<_Tp>& vec, size_t count) const
1851     {
1852         size_t remaining = it->remaining, cn = DataType<_Tp>::channels;
1853         int _fmt = DataType<_Tp>::fmt;
1854         char fmt[] = { (char)((_fmt>>8)+'1'), (char)_fmt, '\0' };
1855         count = std::min(count, remaining/cn);
1856         vec.resize(count);
1857         it->readRaw( string(fmt), (uchar*)&vec[0], count*sizeof(_Tp) );
1858     }
1859     FileNodeIterator* it;
1860 };
1861
1862 template<typename _Tp> static inline void
1863 read( FileNodeIterator& it, vector<_Tp>& vec, size_t maxCount=(size_t)INT_MAX )
1864 {
1865     VecReaderProxy<_Tp, DataType<_Tp>::fmt != 0> r(&it);
1866     r(vec, maxCount);
1867 }
1868
1869 template<typename _Tp> static inline void
1870 read( FileNode& node, vector<_Tp>& vec, const vector<_Tp>& default_value=vector<_Tp>() )
1871 {
1872     read( node.begin(), vec );
1873 }
1874
1875 inline FileNodeIterator FileNode::begin() const
1876 {
1877     return FileNodeIterator(fs, node);
1878 }
1879
1880 inline FileNodeIterator FileNode::end() const
1881 {
1882     return FileNodeIterator(fs, node, size());
1883 }
1884
1885 inline FileNode FileNodeIterator::operator *() const
1886 { return FileNode(fs, (const CvFileNode*)reader.ptr); }
1887
1888 inline FileNode FileNodeIterator::operator ->() const
1889 { return FileNode(fs, (const CvFileNode*)reader.ptr); }
1890
1891 template<typename _Tp> static inline FileNodeIterator& operator >> (FileNodeIterator& it, _Tp& value)
1892 { read( *it, value, _Tp()); return ++it; }
1893
1894 template<typename _Tp> static inline
1895 FileNodeIterator& operator >> (FileNodeIterator& it, vector<_Tp>& vec)
1896 {
1897     VecReaderProxy<_Tp, DataType<_Tp>::fmt != 0> r(&it);
1898     r(vec, (size_t)INT_MAX);
1899     return it;
1900 }
1901
1902 template<typename _Tp> static inline void operator >> (const FileNode& n, _Tp& value)
1903 { FileNodeIterator it = n.begin(); it >> value; }
1904
1905 static inline bool operator == (const FileNodeIterator& it1, const FileNodeIterator& it2)
1906 {
1907     return it1.fs == it2.fs && it1.container == it2.container &&
1908         it1.reader.ptr == it2.reader.ptr && it1.remaining == it2.remaining;
1909 }
1910
1911 static inline bool operator != (const FileNodeIterator& it1, const FileNodeIterator& it2)
1912 {
1913     return !(it1 == it2);
1914 }
1915
1916 static inline ptrdiff_t operator - (const FileNodeIterator& it1, const FileNodeIterator& it2)
1917 {
1918     return it2.remaining - it1.remaining;
1919 }
1920
1921 static inline bool operator < (const FileNodeIterator& it1, const FileNodeIterator& it2)
1922 {
1923     return it1.remaining > it2.remaining;
1924 }
1925
1926 inline FileNode FileStorage::getFirstTopLevelNode() const
1927 {
1928     FileNode r = root();
1929     FileNodeIterator it = r.begin();
1930     return it != r.end() ? *it : FileNode();
1931 }
1932
1933 //////////////////////////////////////// Various algorithms ////////////////////////////////////
1934
1935 template<typename _Tp> static inline _Tp gcd(_Tp a, _Tp b)
1936 {
1937     if( a < b )
1938         std::swap(a, b);
1939     while( b > 0 )
1940     {
1941         _Tp r = a % b;
1942         a = b;
1943         b = r;
1944     }
1945     return a;
1946 }
1947
1948 /****************************************************************************************\
1949
1950   Generic implementation of QuickSort algorithm
1951   Use it as: vector<_Tp> a; ... sort(a,<less_than_predictor>);
1952
1953   The current implementation was derived from *BSD system qsort():
1954
1955     * Copyright (c) 1992, 1993
1956     *  The Regents of the University of California.  All rights reserved.
1957     *
1958     * Redistribution and use in source and binary forms, with or without
1959     * modification, are permitted provided that the following conditions
1960     * are met:
1961     * 1. Redistributions of source code must retain the above copyright
1962     *    notice, this list of conditions and the following disclaimer.
1963     * 2. Redistributions in binary form must reproduce the above copyright
1964     *    notice, this list of conditions and the following disclaimer in the
1965     *    documentation and/or other materials provided with the distribution.
1966     * 3. All advertising materials mentioning features or use of this software
1967     *    must display the following acknowledgement:
1968     *  This product includes software developed by the University of
1969     *  California, Berkeley and its contributors.
1970     * 4. Neither the name of the University nor the names of its contributors
1971     *    may be used to endorse or promote products derived from this software
1972     *    without specific prior written permission.
1973     *
1974     * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
1975     * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
1976     * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
1977     * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
1978     * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
1979     * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
1980     * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
1981     * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
1982     * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
1983     * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
1984     * SUCH DAMAGE.
1985
1986 \****************************************************************************************/
1987
1988 template<typename _Tp, class _LT> void sort( vector<_Tp>& vec, _LT LT=_LT() )
1989 {
1990     int isort_thresh = 7;
1991     int sp = 0;
1992
1993     struct
1994     {
1995         _Tp *lb;
1996         _Tp *ub;
1997     }
1998     stack[48];
1999
2000     size_t total = vec.size();
2001
2002     if( total <= 1 )
2003         return;
2004
2005     _Tp* arr = &vec[0];
2006     stack[0].lb = arr;
2007     stack[0].ub = arr + (total - 1);
2008
2009     while( sp >= 0 )
2010     {
2011         _Tp* left = stack[sp].lb;
2012         _Tp* right = stack[sp--].ub;
2013
2014         for(;;)
2015         {
2016             int i, n = (int)(right - left) + 1, m;
2017             _Tp* ptr;
2018             _Tp* ptr2;
2019
2020             if( n <= isort_thresh )
2021             {
2022             insert_sort:
2023                 for( ptr = left + 1; ptr <= right; ptr++ )
2024                 {
2025                     for( ptr2 = ptr; ptr2 > left && LT(ptr2[0],ptr2[-1]); ptr2--)
2026                         std::swap( ptr2[0], ptr2[-1] );
2027                 }
2028                 break;
2029             }
2030             else
2031             {
2032                 _Tp* left0;
2033                 _Tp* left1;
2034                 _Tp* right0;
2035                 _Tp* right1;
2036                 _Tp* pivot;
2037                 _Tp* a;
2038                 _Tp* b;
2039                 _Tp* c;
2040                 int swap_cnt = 0;
2041
2042                 left0 = left;
2043                 right0 = right;
2044                 pivot = left + (n/2);
2045
2046                 if( n > 40 )
2047                 {
2048                     int d = n / 8;
2049                     a = left, b = left + d, c = left + 2*d;
2050                     left = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))
2051                                       : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));
2052
2053                     a = pivot - d, b = pivot, c = pivot + d;
2054                     pivot = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))
2055                                       : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));
2056
2057                     a = right - 2*d, b = right - d, c = right;
2058                     right = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))
2059                                       : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));
2060                 }
2061
2062                 a = left, b = pivot, c = right;
2063                 pivot = LT(*a, *b) ? (LT(*b, *c) ? b : (LT(*a, *c) ? c : a))
2064                                    : (LT(*c, *b) ? b : (LT(*a, *c) ? a : c));
2065                 if( pivot != left0 )
2066                 {
2067                     std::swap( *pivot, *left0 );
2068                     pivot = left0;
2069                 }
2070                 left = left1 = left0 + 1;
2071                 right = right1 = right0;
2072
2073                 for(;;)
2074                 {
2075                     while( left <= right && !LT(*pivot, *left) )
2076                     {
2077                         if( !LT(*left, *pivot) )
2078                         {
2079                             if( left > left1 )
2080                                 std::swap( *left1, *left );
2081                             swap_cnt = 1;
2082                             left1++;
2083                         }
2084                         left++;
2085                     }
2086
2087                     while( left <= right && !LT(*right, *pivot) )
2088                     {
2089                         if( !LT(*pivot, *right) )
2090                         {
2091                             if( right < right1 )
2092                                 std::swap( *right1, *right );
2093                             swap_cnt = 1;
2094                             right1--;
2095                         }
2096                         right--;
2097                     }
2098
2099                     if( left > right )
2100                         break;
2101                     std::swap( *left, *right );
2102                     swap_cnt = 1;
2103                     left++;
2104                     right--;
2105                 }
2106
2107                 if( swap_cnt == 0 )
2108                 {
2109                     left = left0, right = right0;
2110                     goto insert_sort;
2111                 }
2112
2113                 n = std::min( (int)(left1 - left0), (int)(left - left1) );
2114                 for( i = 0; i < n; i++ )
2115                     std::swap( left0[i], left[i-n] );
2116
2117                 n = std::min( (int)(right0 - right1), (int)(right1 - right) );
2118                 for( i = 0; i < n; i++ )
2119                     std::swap( left[i], right0[i-n+1] );
2120                 n = (int)(left - left1);
2121                 m = (int)(right1 - right);
2122                 if( n > 1 )
2123                 {
2124                     if( m > 1 )
2125                     {
2126                         if( n > m )
2127                         {
2128                             stack[++sp].lb = left0;
2129                             stack[sp].ub = left0 + n - 1;
2130                             left = right0 - m + 1, right = right0;
2131                         }
2132                         else
2133                         {
2134                             stack[++sp].lb = right0 - m + 1;
2135                             stack[sp].ub = right0;
2136                             left = left0, right = left0 + n - 1;
2137                         }
2138                     }
2139                     else
2140                         left = left0, right = left0 + n - 1;
2141                 }
2142                 else if( m > 1 )
2143                     left = right0 - m + 1, right = right0;
2144                 else
2145                     break;
2146             }
2147         }
2148     }
2149 }
2150
2151 template<typename _Tp> class CV_EXPORTS LessThan
2152 {
2153 public:
2154     bool operator()(const _Tp& a, const _Tp& b) const { return a < b; }
2155 };
2156
2157 template<typename _Tp> class CV_EXPORTS GreaterEq
2158 {
2159 public:
2160     bool operator()(const _Tp& a, const _Tp& b) const { return a >= b; }
2161 };
2162
2163 template<typename _Tp> class CV_EXPORTS LessThanIdx
2164 {
2165 public:
2166     LessThanIdx( const _Tp* _arr ) : arr(_arr) {}
2167     bool operator()(int a, int b) const { return arr[a] < arr[b]; }
2168     const _Tp* arr;
2169 };
2170
2171 template<typename _Tp> class CV_EXPORTS GreaterEqIdx
2172 {
2173 public:
2174     GreaterEqIdx( const _Tp* _arr ) : arr(_arr) {}
2175     bool operator()(int a, int b) const { return arr[a] >= arr[b]; }
2176     const _Tp* arr;
2177 };
2178
2179
2180 // This function splits the input sequence or set into one or more equivalence classes and
2181 // returns the vector of labels - 0-based class indexes for each element.
2182 // predicate(a,b) returns true if the two sequence elements certainly belong to the same class.
2183 //
2184 // The algorithm is described in "Introduction to Algorithms"
2185 // by Cormen, Leiserson and Rivest, the chapter "Data structures for disjoint sets"
2186 template<typename _Tp, class _EqPredicate> int
2187 partition( const vector<_Tp>& _vec, vector<int>& labels,
2188            _EqPredicate predicate=_EqPredicate())
2189 {
2190     int i, j, N = (int)_vec.size();
2191     const _Tp* vec = &_vec[0];
2192
2193     const int PARENT=0;
2194     const int RANK=1;
2195
2196     vector<int> _nodes(N*2);
2197     int (*nodes)[2] = (int(*)[2])&_nodes[0];
2198
2199     // The first O(N) pass: create N single-vertex trees
2200     for(i = 0; i < N; i++)
2201     {
2202         nodes[i][PARENT]=-1;
2203         nodes[i][RANK] = 0;
2204     }
2205
2206     // The main O(N^2) pass: merge connected components
2207     for( i = 0; i < N; i++ )
2208     {
2209         int root = i;
2210
2211         // find root
2212         while( nodes[root][PARENT] >= 0 )
2213             root = nodes[root][PARENT];
2214
2215         for( j = 0; j < N; j++ )
2216         {
2217             if( i == j || !predicate(vec[i], vec[j]))
2218                 continue;
2219             int root2 = j;
2220
2221             while( nodes[root2][PARENT] >= 0 )
2222                 root2 = nodes[root2][PARENT];
2223
2224             if( root2 != root )
2225             {
2226                 // unite both trees
2227                 int rank = nodes[root][RANK], rank2 = nodes[root2][RANK];
2228                 if( rank > rank2 )
2229                     nodes[root2][PARENT] = root;
2230                 else
2231                 {
2232                     nodes[root][PARENT] = root2;
2233                     nodes[root2][RANK] += rank == rank2;
2234                     root = root2;
2235                 }
2236                 assert( nodes[root][PARENT] < 0 );
2237
2238                 int k = j, parent;
2239
2240                 // compress the path from node2 to root
2241                 while( (parent = nodes[k][PARENT]) >= 0 )
2242                 {
2243                     nodes[k][PARENT] = root;
2244                     k = parent;
2245                 }
2246
2247                 // compress the path from node to root
2248                 k = i;
2249                 while( (parent = nodes[k][PARENT]) >= 0 )
2250                 {
2251                     nodes[k][PARENT] = root;
2252                     k = parent;
2253                 }
2254             }
2255         }
2256     }
2257
2258     // Final O(N) pass: enumerate classes
2259     labels.resize(N);
2260     int nclasses = 0;
2261
2262     for( i = 0; i < N; i++ )
2263     {
2264         int root = i;
2265         while( nodes[root][PARENT] >= 0 )
2266             root = nodes[root][PARENT];
2267         // re-use the rank as the class label
2268         if( nodes[root][RANK] >= 0 )
2269             nodes[root][RANK] = ~nclasses++;
2270         labels[i] = ~nodes[root][RANK];
2271     }
2272
2273     return nclasses;
2274 }
2275
2276 //////////////////////////////////////////////////////////////////////////////
2277
2278 template<typename _Tp> inline Seq<_Tp>::Seq() : seq(0) {}
2279 template<typename _Tp> inline Seq<_Tp>::Seq( const CvSeq* _seq ) : seq((CvSeq*)_seq)
2280 {
2281     CV_Assert(!_seq || _seq->elem_size == sizeof(_Tp));
2282 }
2283
2284 template<typename _Tp> inline Seq<_Tp>::Seq( MemStorage& storage,
2285                                              int headerSize )
2286 {
2287     CV_Assert(headerSize >= (int)sizeof(CvSeq));
2288     seq = cvCreateSeq(DataType<_Tp>::type, headerSize, sizeof(_Tp), storage);
2289 }
2290
2291 template<typename _Tp> inline _Tp& Seq<_Tp>::operator [](int idx)
2292 { return *(_Tp*)cvGetSeqElem(seq, idx); }
2293
2294 template<typename _Tp> inline const _Tp& Seq<_Tp>::operator [](int idx) const
2295 { return *(_Tp*)cvGetSeqElem(seq, idx); }
2296
2297 template<typename _Tp> inline SeqIterator<_Tp> Seq<_Tp>::begin() const
2298 { return SeqIterator<_Tp>(*this); }
2299
2300 template<typename _Tp> inline SeqIterator<_Tp> Seq<_Tp>::end() const
2301 { return SeqIterator<_Tp>(*this, true); }
2302
2303 template<typename _Tp> inline size_t Seq<_Tp>::size() const
2304 { return seq ? seq->total : 0; }
2305
2306 template<typename _Tp> inline int Seq<_Tp>::type() const
2307 { return seq ? CV_MAT_TYPE(seq->flags) : 0; }
2308
2309 template<typename _Tp> inline int Seq<_Tp>::depth() const
2310 { return seq ? CV_MAT_DEPTH(seq->flags) : 0; }
2311
2312 template<typename _Tp> inline int Seq<_Tp>::channels() const
2313 { return seq ? CV_MAT_CN(seq->flags) : 0; }
2314
2315 template<typename _Tp> inline size_t Seq<_Tp>::elemSize() const
2316 { return seq ? seq->elem_size : 0; }
2317
2318 template<typename _Tp> inline size_t Seq<_Tp>::index(const _Tp& elem) const
2319 { return cvSeqElemIdx(seq, &elem); }
2320
2321 template<typename _Tp> inline void Seq<_Tp>::push_back(const _Tp& elem)
2322 { cvSeqPush(seq, &elem); }
2323
2324 template<typename _Tp> inline void Seq<_Tp>::push_front(const _Tp& elem)
2325 { cvSeqPushFront(seq, &elem); }
2326
2327 template<typename _Tp> inline void Seq<_Tp>::push_back(const _Tp* elem, size_t count)
2328 { cvSeqPushMulti(seq, elem, (int)count, 0); }
2329
2330 template<typename _Tp> inline void Seq<_Tp>::push_front(const _Tp* elem, size_t count)
2331 { cvSeqPushMulti(seq, elem, (int)count, 1); }
2332
2333 template<typename _Tp> inline _Tp& Seq<_Tp>::back()
2334 { return *(_Tp*)cvGetSeqElem(seq, -1); }
2335
2336 template<typename _Tp> inline const _Tp& Seq<_Tp>::back() const
2337 { return *(const _Tp*)cvGetSeqElem(seq, -1); }
2338
2339 template<typename _Tp> inline _Tp& Seq<_Tp>::front()
2340 { return *(_Tp*)cvGetSeqElem(seq, 0); }
2341
2342 template<typename _Tp> inline const _Tp& Seq<_Tp>::front() const
2343 { return *(const _Tp*)cvGetSeqElem(seq, 0); }
2344
2345 template<typename _Tp> inline bool Seq<_Tp>::empty() const
2346 { return !seq || seq->total == 0; }
2347
2348 template<typename _Tp> inline void Seq<_Tp>::clear()
2349 { if(seq) cvClearSeq(seq); }
2350
2351 template<typename _Tp> inline void Seq<_Tp>::pop_back()
2352 { cvSeqPop(seq); }
2353
2354 template<typename _Tp> inline void Seq<_Tp>::pop_front()
2355 { cvSeqPopFront(seq); }
2356
2357 template<typename _Tp> inline void Seq<_Tp>::pop_back(_Tp* elem, size_t count)
2358 { cvSeqPopMulti(seq, elem, (int)count, 0); }
2359
2360 template<typename _Tp> inline void Seq<_Tp>::pop_front(_Tp* elem, size_t count)
2361 { cvSeqPopMulti(seq, elem, (int)count, 1); }
2362
2363 template<typename _Tp> inline void Seq<_Tp>::insert(int idx, const _Tp& elem)
2364 { cvSeqInsert(seq, idx, &elem); }
2365
2366 template<typename _Tp> inline void Seq<_Tp>::insert(int idx, const _Tp* elems, size_t count)
2367 {
2368     CvMat m = cvMat(1, count, DataType<_Tp>::type, elems);
2369     cvSeqInsertSlice(seq, idx, &m);
2370 }
2371
2372 template<typename _Tp> inline void Seq<_Tp>::remove(int idx)
2373 { cvSeqRemove(seq, idx); }
2374
2375 template<typename _Tp> inline void Seq<_Tp>::remove(const Range& r)
2376 { cvSeqRemoveSlice(seq, r); }
2377
2378 template<typename _Tp> inline void Seq<_Tp>::copyTo(vector<_Tp>& vec, const Range& range) const
2379 {
2380     size_t len = !seq ? 0 : range == Range::all() ? seq->total : range.end - range.start;
2381     vec.resize(len);
2382     if( seq && len )
2383         cvCvtSeqToArray(seq, &vec[0], range);
2384 }
2385
2386 template<typename _Tp> inline Seq<_Tp>::operator vector<_Tp>() const
2387 {
2388     vector<_Tp> vec;
2389     copyTo(vec);
2390     return vec;
2391 }
2392
2393 template<typename _Tp> inline SeqIterator<_Tp>::SeqIterator()
2394 { memset(this, 0, sizeof(*this)); }
2395
2396 template<typename _Tp> inline SeqIterator<_Tp>::SeqIterator(const Seq<_Tp>& seq, bool seekEnd)
2397 {
2398     cvStartReadSeq(seq.seq, this);
2399     if( seekEnd )
2400         index = seq.seq->total;
2401 }
2402
2403 template<typename _Tp> inline void SeqIterator<_Tp>::seek(size_t pos)
2404 {
2405     cvSetSeqReaderPos(this, (int)pos, false);
2406     index = pos;
2407 }
2408
2409 template<typename _Tp> inline size_t SeqIterator<_Tp>::tell() const
2410 { return index; }
2411
2412 template<typename _Tp> inline _Tp& SeqIterator<_Tp>::operator *()
2413 { return *(_Tp*)ptr; }
2414
2415 template<typename _Tp> inline const _Tp& SeqIterator<_Tp>::operator *() const
2416 { return *(const _Tp*)ptr; }
2417
2418 template<typename _Tp> inline SeqIterator<_Tp>& SeqIterator<_Tp>::operator ++()
2419 {
2420     CV_NEXT_SEQ_ELEM(sizeof(_Tp), *this);
2421     if( ++index >= seq->total*2 )
2422         index = 0;
2423     return *this;
2424 }
2425
2426 template<typename _Tp> inline SeqIterator<_Tp> SeqIterator<_Tp>::operator ++(int) const
2427 {
2428     SeqIterator<_Tp> it = *this;
2429     ++*this;
2430     return it;
2431 }
2432
2433 template<typename _Tp> inline SeqIterator<_Tp>& SeqIterator<_Tp>::operator --()
2434 {
2435     CV_PREV_SEQ_ELEM(sizeof(_Tp), *this);
2436     if( --index < 0 )
2437         index = seq->total*2-1;
2438     return *this;
2439 }
2440
2441 template<typename _Tp> inline SeqIterator<_Tp> SeqIterator<_Tp>::operator --(int) const
2442 {
2443     SeqIterator<_Tp> it = *this;
2444     --*this;
2445     return it;
2446 }
2447
2448 template<typename _Tp> inline SeqIterator<_Tp>& SeqIterator<_Tp>::operator +=(int delta)
2449 {
2450     cvSetSeqReaderPos(this, delta, 1);
2451     index += delta;
2452     int n = seq->total*2;
2453     if( index < 0 )
2454         index += n;
2455     if( index >= n )
2456         index -= n;
2457     return *this;
2458 }
2459
2460 template<typename _Tp> inline SeqIterator<_Tp>& SeqIterator<_Tp>::operator -=(int delta)
2461 {
2462     return (*this += -delta);
2463 }
2464
2465 template<typename _Tp> inline ptrdiff_t operator - (const SeqIterator<_Tp>& a,
2466                                                     const SeqIterator<_Tp>& b)
2467 {
2468     ptrdiff_t delta = a.index - b.index, n = a.seq->total;
2469     if( std::abs(delta) > n )
2470         delta += delta < 0 ? n : -n;
2471     return delta;
2472 }
2473
2474 template<typename _Tp> inline bool operator == (const SeqIterator<_Tp>& a,
2475                                                 const SeqIterator<_Tp>& b)
2476 {
2477     return a.seq == b.seq && a.index == b.index;
2478 }
2479
2480 template<typename _Tp> inline bool operator != (const SeqIterator<_Tp>& a,
2481                                                 const SeqIterator<_Tp>& b)
2482 {
2483     return !(a == b);
2484 }
2485
2486 }
2487
2488 #endif // __cplusplus
2489 #endif // _OPENCV_CORE_OPERATIONS_H_