Update to 2.0.0 tree from current Fremantle build
[opencv] / src / cv / cvmorph.cpp
1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 //  By downloading, copying, installing or using the software you agree to this license.
6 //  If you do not agree to this license, do not download, install,
7 //  copy or use the software.
8 //
9 //
10 //                           License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 //   * Redistribution's of source code must retain the above copyright notice,
21 //     this list of conditions and the following disclaimer.
22 //
23 //   * Redistribution's in binary form must reproduce the above copyright notice,
24 //     this list of conditions and the following disclaimer in the documentation
25 //     and/or other materials provided with the distribution.
26 //
27 //   * The name of the copyright holders may not be used to endorse or promote products
28 //     derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 #include "_cv.h"
44 #include <limits.h>
45 #include <stdio.h>
46
47 /****************************************************************************************\
48                      Basic Morphological Operations: Erosion & Dilation
49 \****************************************************************************************/
50
51 namespace cv
52 {
53
54 template<typename T> struct MinOp
55 {
56     typedef T type1;
57     typedef T type2;
58     typedef T rtype;
59     T operator ()(T a, T b) const { return std::min(a, b); }
60 };
61
62 template<typename T> struct MaxOp
63 {
64     typedef T type1;
65     typedef T type2;
66     typedef T rtype;
67     T operator ()(T a, T b) const { return std::max(a, b); }
68 };
69
70 #undef CV_MIN_8U
71 #undef CV_MAX_8U
72 #define CV_MIN_8U(a,b)       ((a) - CV_FAST_CAST_8U((a) - (b)))
73 #define CV_MAX_8U(a,b)       ((a) + CV_FAST_CAST_8U((b) - (a)))
74
75 template<> inline uchar MinOp<uchar>::operator ()(uchar a, uchar b) const { return CV_MIN_8U(a, b); }
76 template<> inline uchar MaxOp<uchar>::operator ()(uchar a, uchar b) const { return CV_MAX_8U(a, b); }
77
78 #if CV_SSE2
79
80 template<class VecUpdate> struct MorphRowIVec
81 {
82     enum { ESZ = VecUpdate::ESZ };
83
84     MorphRowIVec(int _ksize, int _anchor) : ksize(_ksize), anchor(_anchor) {}
85     int operator()(const uchar* src, uchar* dst, int width, int cn) const
86     {
87         cn *= ESZ;
88         int i, k, _ksize = ksize*cn;
89         width *= cn;
90         VecUpdate updateOp;
91
92         for( i = 0; i <= width - 16; i += 16 )
93         {
94             __m128i s = _mm_loadu_si128((const __m128i*)(src + i));
95             for( k = cn; k < _ksize; k += cn )
96             {
97                 __m128i x = _mm_loadu_si128((const __m128i*)(src + i + k));
98                 s = updateOp(s, x);
99             }
100             _mm_storeu_si128((__m128i*)(dst + i), s);
101         }
102
103         for( ; i <= width - 4; i += 4 )
104         {
105             __m128i s = _mm_cvtsi32_si128(*(const int*)(src + i));
106             for( k = cn; k < _ksize; k += cn )
107             {
108                 __m128i x = _mm_cvtsi32_si128(*(const int*)(src + i + k));
109                 s = updateOp(s, x);
110             }
111             *(int*)(dst + i) = _mm_cvtsi128_si32(s);
112         }
113
114         return i/ESZ;
115     }
116
117     int ksize, anchor;
118 };
119
120
121 template<class VecUpdate> struct MorphRowFVec
122 {
123     MorphRowFVec(int _ksize, int _anchor) : ksize(_ksize), anchor(_anchor) {}
124     int operator()(const uchar* src, uchar* dst, int width, int cn) const
125     {
126         int i, k, _ksize = ksize*cn;
127         width *= cn;
128         VecUpdate updateOp;
129
130         for( i = 0; i <= width - 4; i += 4 )
131         {
132             __m128 s = _mm_loadu_ps((const float*)src + i);
133             for( k = cn; k < _ksize; k += cn )
134             {
135                 __m128 x = _mm_loadu_ps((const float*)src + i + k);
136                 s = updateOp(s, x);
137             }
138             _mm_storeu_ps((float*)dst + i, s);
139         }
140
141         return i;
142     }
143
144     int ksize, anchor;
145 };
146
147
148 template<class VecUpdate> struct MorphColumnIVec
149 {
150     enum { ESZ = VecUpdate::ESZ };
151
152     MorphColumnIVec(int _ksize, int _anchor) : ksize(_ksize), anchor(_anchor) {}
153     int operator()(const uchar** src, uchar* dst, int dststep, int count, int width) const
154     {
155         int i = 0, k, _ksize = ksize;
156         width *= ESZ;
157         VecUpdate updateOp;
158
159         for( i = 0; i < count + ksize - 1; i++ )
160             CV_Assert( ((size_t)src[i] & 15) == 0 );
161
162         for( ; _ksize > 1 && count > 1; count -= 2, dst += dststep*2, src += 2 )
163         {
164             for( i = 0; i <= width - 32; i += 32 )
165             {
166                 const uchar* sptr = src[1] + i;
167                 __m128i s0 = _mm_load_si128((const __m128i*)sptr);
168                 __m128i s1 = _mm_load_si128((const __m128i*)(sptr + 16));
169                 __m128i x0, x1;
170
171                 for( k = 2; k < _ksize; k++ )
172                 {
173                     sptr = src[k] + i;
174                     x0 = _mm_load_si128((const __m128i*)sptr);
175                     x1 = _mm_load_si128((const __m128i*)(sptr + 16));
176                     s0 = updateOp(s0, x0);
177                     s1 = updateOp(s1, x1);
178                 }
179
180                 sptr = src[0] + i;
181                 x0 = _mm_load_si128((const __m128i*)sptr);
182                 x1 = _mm_load_si128((const __m128i*)(sptr + 16));
183                 _mm_storeu_si128((__m128i*)(dst + i), updateOp(s0, x0));
184                 _mm_storeu_si128((__m128i*)(dst + i + 16), updateOp(s1, x1));
185
186                 sptr = src[k] + i;
187                 x0 = _mm_load_si128((const __m128i*)sptr);
188                 x1 = _mm_load_si128((const __m128i*)(sptr + 16));
189                 _mm_storeu_si128((__m128i*)(dst + dststep + i), updateOp(s0, x0));
190                 _mm_storeu_si128((__m128i*)(dst + dststep + i + 16), updateOp(s1, x1));
191             }
192
193             for( ; i <= width - 8; i += 8 )
194             {
195                 __m128i s0 = _mm_loadl_epi64((const __m128i*)(src[1] + i)), x0;
196
197                 for( k = 2; k < _ksize; k++ )
198                 {
199                     x0 = _mm_loadl_epi64((const __m128i*)(src[k] + i));
200                     s0 = updateOp(s0, x0);
201                 }
202
203                 x0 = _mm_loadl_epi64((const __m128i*)(src[0] + i));
204                 _mm_storel_epi64((__m128i*)(dst + i), updateOp(s0, x0));
205                 x0 = _mm_loadl_epi64((const __m128i*)(src[k] + i));
206                 _mm_storel_epi64((__m128i*)(dst + dststep + i), updateOp(s0, x0));
207             }
208         }
209
210         for( ; count > 0; count--, dst += dststep, src++ )
211         {
212             for( i = 0; i <= width - 32; i += 32 )
213             {
214                 const uchar* sptr = src[0] + i;
215                 __m128i s0 = _mm_load_si128((const __m128i*)sptr);
216                 __m128i s1 = _mm_load_si128((const __m128i*)(sptr + 16));
217                 __m128i x0, x1;
218
219                 for( k = 1; k < _ksize; k++ )
220                 {
221                     sptr = src[k] + i;
222                     x0 = _mm_load_si128((const __m128i*)sptr);
223                     x1 = _mm_load_si128((const __m128i*)(sptr + 16));
224                     s0 = updateOp(s0, x0);
225                     s1 = updateOp(s1, x1);
226                 }
227                 _mm_storeu_si128((__m128i*)(dst + i), s0);
228                 _mm_storeu_si128((__m128i*)(dst + i + 16), s1);
229             }
230
231             for( ; i <= width - 8; i += 8 )
232             {
233                 __m128i s0 = _mm_loadl_epi64((const __m128i*)(src[0] + i)), x0;
234
235                 for( k = 1; k < _ksize; k++ )
236                 {
237                     x0 = _mm_loadl_epi64((const __m128i*)(src[k] + i));
238                     s0 = updateOp(s0, x0);
239                 }
240                 _mm_storel_epi64((__m128i*)(dst + i), s0);
241             }
242         }
243
244         return i/ESZ;
245     }
246
247     int ksize, anchor;
248 };
249
250
251 template<class VecUpdate> struct MorphColumnFVec
252 {
253     MorphColumnFVec(int _ksize, int _anchor) : ksize(_ksize), anchor(_anchor) {}
254     int operator()(const uchar** _src, uchar* _dst, int dststep, int count, int width) const
255     {
256         int i = 0, k, _ksize = ksize;
257         VecUpdate updateOp;
258
259         for( i = 0; i < count + ksize - 1; i++ )
260             CV_Assert( ((size_t)_src[i] & 15) == 0 );
261
262         const float** src = (const float**)_src;
263         float* dst = (float*)_dst;
264         dststep /= sizeof(dst[0]);
265
266         for( ; _ksize > 1 && count > 1; count -= 2, dst += dststep*2, src += 2 )
267         {
268             for( i = 0; i <= width - 16; i += 16 )
269             {
270                 const float* sptr = src[1] + i;
271                 __m128 s0 = _mm_load_ps(sptr);
272                 __m128 s1 = _mm_load_ps(sptr + 4);
273                 __m128 s2 = _mm_load_ps(sptr + 8);
274                 __m128 s3 = _mm_load_ps(sptr + 12);
275                 __m128 x0, x1, x2, x3;
276
277                 for( k = 2; k < _ksize; k++ )
278                 {
279                     sptr = src[k] + i;
280                     x0 = _mm_load_ps(sptr);
281                     x1 = _mm_load_ps(sptr + 4);
282                     s0 = updateOp(s0, x0);
283                     s1 = updateOp(s1, x1);
284                     x2 = _mm_load_ps(sptr + 8);
285                     x3 = _mm_load_ps(sptr + 12);
286                     s2 = updateOp(s2, x2);
287                     s3 = updateOp(s3, x3);
288                 }
289
290                 sptr = src[0] + i;
291                 x0 = _mm_load_ps(sptr);
292                 x1 = _mm_load_ps(sptr + 4);
293                 x2 = _mm_load_ps(sptr + 8);
294                 x3 = _mm_load_ps(sptr + 12);
295                 _mm_storeu_ps(dst + i, updateOp(s0, x0));
296                 _mm_storeu_ps(dst + i + 4, updateOp(s1, x1));
297                 _mm_storeu_ps(dst + i + 8, updateOp(s2, x2));
298                 _mm_storeu_ps(dst + i + 12, updateOp(s3, x3));
299
300                 sptr = src[k] + i;
301                 x0 = _mm_load_ps(sptr);
302                 x1 = _mm_load_ps(sptr + 4);
303                 x2 = _mm_load_ps(sptr + 8);
304                 x3 = _mm_load_ps(sptr + 12);
305                 _mm_storeu_ps(dst + dststep + i, updateOp(s0, x0));
306                 _mm_storeu_ps(dst + dststep + i + 4, updateOp(s1, x1));
307                 _mm_storeu_ps(dst + dststep + i + 8, updateOp(s2, x2));
308                 _mm_storeu_ps(dst + dststep + i + 12, updateOp(s3, x3));
309             }
310
311             for( ; i <= width - 4; i += 4 )
312             {
313                 __m128 s0 = _mm_load_ps(src[1] + i), x0;
314
315                 for( k = 2; k < _ksize; k++ )
316                 {
317                     x0 = _mm_load_ps(src[k] + i);
318                     s0 = updateOp(s0, x0);
319                 }
320
321                 x0 = _mm_load_ps(src[0] + i);
322                 _mm_storeu_ps(dst + i, updateOp(s0, x0));
323                 x0 = _mm_load_ps(src[k] + i);
324                 _mm_storeu_ps(dst + dststep + i, updateOp(s0, x0));
325             }
326         }
327
328         for( ; count > 0; count--, dst += dststep, src++ )
329         {
330             for( i = 0; i <= width - 16; i += 16 )
331             {
332                 const float* sptr = src[0] + i;
333                 __m128 s0 = _mm_load_ps(sptr);
334                 __m128 s1 = _mm_load_ps(sptr + 4);
335                 __m128 s2 = _mm_load_ps(sptr + 8);
336                 __m128 s3 = _mm_load_ps(sptr + 12);
337                 __m128 x0, x1, x2, x3;
338
339                 for( k = 1; k < _ksize; k++ )
340                 {
341                     sptr = src[k] + i;
342                     x0 = _mm_load_ps(sptr);
343                     x1 = _mm_load_ps(sptr + 4);
344                     s0 = updateOp(s0, x0);
345                     s1 = updateOp(s1, x1);
346                     x2 = _mm_load_ps(sptr + 8);
347                     x3 = _mm_load_ps(sptr + 12);
348                     s2 = updateOp(s2, x2);
349                     s3 = updateOp(s3, x3);
350                 }
351                 _mm_storeu_ps(dst + i, s0);
352                 _mm_storeu_ps(dst + i + 4, s1);
353                 _mm_storeu_ps(dst + i + 8, s2);
354                 _mm_storeu_ps(dst + i + 12, s3);
355             }
356
357             for( i = 0; i <= width - 4; i += 4 )
358             {
359                 __m128 s0 = _mm_load_ps(src[0] + i), x0;
360                 for( k = 1; k < _ksize; k++ )
361                 {
362                     x0 = _mm_load_ps(src[k] + i);
363                     s0 = updateOp(s0, x0);
364                 }
365                 _mm_storeu_ps(dst + i, s0);
366             }
367         }
368
369         return i;
370     }
371
372     int ksize, anchor;
373 };
374
375
376 template<class VecUpdate> struct MorphIVec
377 {
378     enum { ESZ = VecUpdate::ESZ };
379
380     int operator()(uchar** src, int nz, uchar* dst, int width) const
381     {
382         int i, k;
383         width *= ESZ;
384         VecUpdate updateOp;
385
386         for( i = 0; i <= width - 32; i += 32 )
387         {
388             const uchar* sptr = src[0] + i;
389             __m128i s0 = _mm_loadu_si128((const __m128i*)sptr);
390             __m128i s1 = _mm_loadu_si128((const __m128i*)(sptr + 16));
391             __m128i x0, x1;
392
393             for( k = 1; k < nz; k++ )
394             {
395                 sptr = src[k] + i;
396                 x0 = _mm_loadu_si128((const __m128i*)sptr);
397                 x1 = _mm_loadu_si128((const __m128i*)(sptr + 16));
398                 s0 = updateOp(s0, x0);
399                 s1 = updateOp(s1, x1);
400             }
401             _mm_storeu_si128((__m128i*)(dst + i), s0);
402             _mm_storeu_si128((__m128i*)(dst + i + 16), s1);
403         }
404
405         for( ; i <= width - 8; i += 8 )
406         {
407             __m128i s0 = _mm_loadl_epi64((const __m128i*)(src[0] + i)), x0;
408
409             for( k = 1; k < nz; k++ )
410             {
411                 x0 = _mm_loadl_epi64((const __m128i*)(src[k] + i));
412                 s0 = updateOp(s0, x0);
413             }
414             _mm_storel_epi64((__m128i*)(dst + i), s0);
415         }
416
417         return i/ESZ;
418     }
419 };
420
421
422 template<class VecUpdate> struct MorphFVec
423 {
424     int operator()(uchar** _src, int nz, uchar* _dst, int width) const
425     {
426         const float** src = (const float**)_src;
427         float* dst = (float*)_dst;
428         int i, k;
429         VecUpdate updateOp;
430
431         for( i = 0; i <= width - 16; i += 16 )
432         {
433             const float* sptr = src[0] + i;
434             __m128 s0 = _mm_loadu_ps(sptr);
435             __m128 s1 = _mm_loadu_ps(sptr + 4);
436             __m128 s2 = _mm_loadu_ps(sptr + 8);
437             __m128 s3 = _mm_loadu_ps(sptr + 12);
438             __m128 x0, x1, x2, x3;
439
440             for( k = 1; k < nz; k++ )
441             {
442                 sptr = src[k] + i;
443                 x0 = _mm_loadu_ps(sptr);
444                 x1 = _mm_loadu_ps(sptr + 4);
445                 x2 = _mm_loadu_ps(sptr + 8);
446                 x3 = _mm_loadu_ps(sptr + 12);
447                 s0 = updateOp(s0, x0);
448                 s1 = updateOp(s1, x1);
449                 s2 = updateOp(s2, x2);
450                 s3 = updateOp(s3, x3);
451             }
452             _mm_storeu_ps(dst + i, s0);
453             _mm_storeu_ps(dst + i + 4, s1);
454             _mm_storeu_ps(dst + i + 8, s2);
455             _mm_storeu_ps(dst + i + 12, s3);
456         }
457
458         for( ; i <= width - 4; i += 4 )
459         {
460             __m128 s0 = _mm_loadu_ps(src[0] + i), x0;
461
462             for( k = 1; k < nz; k++ )
463             {
464                 x0 = _mm_loadu_ps(src[k] + i);
465                 s0 = updateOp(s0, x0);
466             }
467             _mm_storeu_ps(dst + i, s0);
468         }
469
470         for( ; i < width; i++ )
471         {
472             __m128 s0 = _mm_load_ss(src[0] + i), x0;
473
474             for( k = 1; k < nz; k++ )
475             {
476                 x0 = _mm_load_ss(src[k] + i);
477                 s0 = updateOp(s0, x0);
478             }
479             _mm_store_ss(dst + i, s0);
480         }
481
482         return i;
483     }
484 };
485
486
487 struct VMin8u
488 {
489     enum { ESZ = 1 };
490     __m128i operator()(const __m128i& a, const __m128i& b) const { return _mm_min_epu8(a,b); }
491 };
492 struct VMax8u
493 {
494     enum { ESZ = 1 };
495     __m128i operator()(const __m128i& a, const __m128i& b) const { return _mm_max_epu8(a,b); }
496 };
497 struct VMin16u
498 {
499     enum { ESZ = 2 };
500     __m128i operator()(const __m128i& a, const __m128i& b) const
501     { return _mm_subs_epu16(a,_mm_subs_epu16(a,b)); }
502 };
503 struct VMax16u
504 {
505     enum { ESZ = 2 };
506     __m128i operator()(const __m128i& a, const __m128i& b) const
507     { return _mm_adds_epu16(_mm_subs_epu16(a,b),b); }
508 };
509 struct VMin16s
510 {
511     enum { ESZ = 2 };
512     __m128i operator()(const __m128i& a, const __m128i& b) const
513     { return _mm_min_epi16(a, b); }
514 };
515 struct VMax16s
516 {
517     enum { ESZ = 2 };
518     __m128i operator()(const __m128i& a, const __m128i& b) const
519     { return _mm_max_epi16(a, b); }
520 };
521 struct VMin32f { __m128 operator()(const __m128& a, const __m128& b) const { return _mm_min_ps(a,b); }};
522 struct VMax32f { __m128 operator()(const __m128& a, const __m128& b) const { return _mm_max_ps(a,b); }};
523
524 typedef MorphRowIVec<VMin8u> ErodeRowVec8u;
525 typedef MorphRowIVec<VMax8u> DilateRowVec8u;
526 typedef MorphRowIVec<VMin16u> ErodeRowVec16u;
527 typedef MorphRowIVec<VMax16u> DilateRowVec16u;
528 typedef MorphRowIVec<VMin16s> ErodeRowVec16s;
529 typedef MorphRowIVec<VMax16s> DilateRowVec16s;
530 typedef MorphRowFVec<VMin32f> ErodeRowVec32f;
531 typedef MorphRowFVec<VMax32f> DilateRowVec32f;
532
533 typedef MorphColumnIVec<VMin8u> ErodeColumnVec8u;
534 typedef MorphColumnIVec<VMax8u> DilateColumnVec8u;
535 typedef MorphColumnIVec<VMin16u> ErodeColumnVec16u;
536 typedef MorphColumnIVec<VMax16u> DilateColumnVec16s;
537 typedef MorphColumnIVec<VMin16s> ErodeColumnVec16s;
538 typedef MorphColumnIVec<VMax16s> DilateColumnVec16u;
539 typedef MorphColumnFVec<VMin32f> ErodeColumnVec32f;
540 typedef MorphColumnFVec<VMax32f> DilateColumnVec32f;
541
542 typedef MorphIVec<VMin8u> ErodeVec8u;
543 typedef MorphIVec<VMax8u> DilateVec8u;
544 typedef MorphIVec<VMin16u> ErodeVec16u;
545 typedef MorphIVec<VMax16u> DilateVec16u;
546 typedef MorphIVec<VMin16s> ErodeVec16s;
547 typedef MorphIVec<VMax16s> DilateVec16s;
548 typedef MorphFVec<VMin32f> ErodeVec32f;
549 typedef MorphFVec<VMax32f> DilateVec32f;
550
551 #else
552
553 struct MorphRowNoVec
554 {
555     MorphRowNoVec(int, int) {}
556     int operator()(const uchar*, uchar*, int, int) const { return 0; }
557 };
558
559 struct MorphColumnNoVec
560 {
561     MorphColumnNoVec(int, int) {}
562     int operator()(const uchar**, uchar*, int, int, int) const { return 0; }
563 };
564
565 struct MorphNoVec
566 {
567     int operator()(uchar**, int, uchar*, int) const { return 0; }
568 };
569
570 typedef MorphRowNoVec ErodeRowVec8u;
571 typedef MorphRowNoVec DilateRowVec8u;
572 typedef MorphRowNoVec ErodeRowVec16u;
573 typedef MorphRowNoVec DilateRowVec16u;
574 typedef MorphRowNoVec ErodeRowVec16s;
575 typedef MorphRowNoVec DilateRowVec16s;
576 typedef MorphRowNoVec ErodeRowVec32f;
577 typedef MorphRowNoVec DilateRowVec32f;
578
579 typedef MorphColumnNoVec ErodeColumnVec8u;
580 typedef MorphColumnNoVec DilateColumnVec8u;
581 typedef MorphColumnNoVec ErodeColumnVec16u;
582 typedef MorphColumnNoVec DilateColumnVec16u;
583 typedef MorphColumnNoVec ErodeColumnVec16s;
584 typedef MorphColumnNoVec DilateColumnVec16s;
585 typedef MorphColumnNoVec ErodeColumnVec32f;
586 typedef MorphColumnNoVec DilateColumnVec32f;
587
588 typedef MorphNoVec ErodeVec8u;
589 typedef MorphNoVec DilateVec8u;
590 typedef MorphNoVec ErodeVec16u;
591 typedef MorphNoVec DilateVec16u;
592 typedef MorphNoVec ErodeVec16s;
593 typedef MorphNoVec DilateVec16s;
594 typedef MorphNoVec ErodeVec32f;
595 typedef MorphNoVec DilateVec32f;
596
597 #endif
598
599 template<class Op, class VecOp> struct MorphRowFilter : public BaseRowFilter
600 {
601     typedef typename Op::rtype T;
602
603     MorphRowFilter( int _ksize, int _anchor ) : vecOp(_ksize, _anchor)
604     {
605         ksize = _ksize;
606         anchor = _anchor;
607     }
608
609     void operator()(const uchar* src, uchar* dst, int width, int cn)
610     {
611         int i, j, k, _ksize = ksize*cn;
612         const T* S = (const T*)src;
613         Op op;
614         T* D = (T*)dst;
615
616         if( _ksize == cn )
617         {
618             for( i = 0; i < width*cn; i++ )
619                 D[i] = S[i];
620             return;
621         }
622
623         int i0 = vecOp(src, dst, width, cn);
624         width *= cn;
625
626         for( k = 0; k < cn; k++, S++, D++ )
627         {
628             for( i = i0; i <= width - cn*2; i += cn*2 )
629             {
630                 const T* s = S + i;
631                 T m = s[cn];
632                 for( j = cn*2; j < _ksize; j += cn )
633                     m = op(m, s[j]);
634                 D[i] = op(m, s[0]);
635                 D[i+cn] = op(m, s[j]);
636             }
637
638             for( ; i < width; i += cn )
639             {
640                 const T* s = S + i;
641                 T m = s[0];
642                 for( j = cn; j < _ksize; j += cn )
643                     m = op(m, s[j]);
644                 D[i] = m;
645             }
646         }
647     }
648
649     VecOp vecOp;
650 };
651
652
653 template<class Op, class VecOp> struct MorphColumnFilter : public BaseColumnFilter
654 {
655     typedef typename Op::rtype T;
656
657     MorphColumnFilter( int _ksize, int _anchor ) : vecOp(_ksize, _anchor)
658     {
659         ksize = _ksize;
660         anchor = _anchor;
661     }
662
663     void operator()(const uchar** _src, uchar* dst, int dststep, int count, int width)
664     {
665         int i, k, _ksize = ksize;
666         const T** src = (const T**)_src;
667         T* D = (T*)dst;
668         Op op;
669
670         int i0 = vecOp(_src, dst, dststep, count, width);
671         dststep /= sizeof(D[0]);
672
673         for( ; _ksize > 1 && count > 1; count -= 2, D += dststep*2, src += 2 )
674         {
675             for( i = i0; i <= width - 4; i += 4 )
676             {
677                 const T* sptr = src[1] + i;
678                 T s0 = sptr[0], s1 = sptr[1], s2 = sptr[2], s3 = sptr[3];
679
680                 for( k = 2; k < _ksize; k++ )
681                 {
682                     sptr = src[k] + i;
683                     s0 = op(s0, sptr[0]); s1 = op(s1, sptr[1]);
684                     s2 = op(s2, sptr[2]); s3 = op(s3, sptr[3]);
685                 }
686
687                 sptr = src[0] + i;
688                 D[i] = op(s0, sptr[0]);
689                 D[i+1] = op(s1, sptr[1]);
690                 D[i+2] = op(s2, sptr[2]);
691                 D[i+3] = op(s3, sptr[3]);
692
693                 sptr = src[k] + i;
694                 D[i+dststep] = op(s0, sptr[0]);
695                 D[i+dststep+1] = op(s1, sptr[1]);
696                 D[i+dststep+2] = op(s2, sptr[2]);
697                 D[i+dststep+3] = op(s3, sptr[3]);
698             }
699
700             for( ; i < width; i++ )
701             {
702                 T s0 = src[1][i];
703
704                 for( k = 2; k < _ksize; k++ )
705                     s0 = op(s0, src[k][i]);
706
707                 D[i] = op(s0, src[0][i]);
708                 D[i+dststep] = op(s0, src[k][i]);
709             }
710         }
711
712         for( ; count > 0; count--, D += dststep, src++ )
713         {
714             for( i = i0; i <= width - 4; i += 4 )
715             {
716                 const T* sptr = src[0] + i;
717                 T s0 = sptr[0], s1 = sptr[1], s2 = sptr[2], s3 = sptr[3];
718
719                 for( k = 1; k < _ksize; k++ )
720                 {
721                     sptr = src[k] + i;
722                     s0 = op(s0, sptr[0]); s1 = op(s1, sptr[1]);
723                     s2 = op(s2, sptr[2]); s3 = op(s3, sptr[3]);
724                 }
725
726                 D[i] = s0; D[i+1] = s1;
727                 D[i+2] = s2; D[i+3] = s3;
728             }
729
730             for( ; i < width; i++ )
731             {
732                 T s0 = src[0][i];
733                 for( k = 1; k < _ksize; k++ )
734                     s0 = op(s0, src[k][i]);
735                 D[i] = s0;
736             }
737         }
738     }
739
740     VecOp vecOp;
741 };
742
743
744 template<class Op, class VecOp> struct MorphFilter : BaseFilter
745 {
746     typedef typename Op::rtype T;
747
748     MorphFilter( const Mat& _kernel, Point _anchor )
749     {
750         anchor = _anchor;
751         ksize = _kernel.size();
752         CV_Assert( _kernel.type() == CV_8U );
753
754         vector<uchar> coeffs; // we do not really the values of non-zero
755                               // kernel elements, just their locations
756         preprocess2DKernel( _kernel, coords, coeffs );
757         ptrs.resize( coords.size() );
758     }
759
760     void operator()(const uchar** src, uchar* dst, int dststep, int count, int width, int cn)
761     {
762         const Point* pt = &coords[0];
763         const T** kp = (const T**)&ptrs[0];
764         int i, k, nz = (int)coords.size();
765         Op op;
766
767         width *= cn;
768         for( ; count > 0; count--, dst += dststep, src++ )
769         {
770             T* D = (T*)dst;
771
772             for( k = 0; k < nz; k++ )
773                 kp[k] = (const T*)src[pt[k].y] + pt[k].x*cn;
774
775             i = vecOp(&ptrs[0], nz, dst, width);
776
777             for( ; i <= width - 4; i += 4 )
778             {
779                 const T* sptr = kp[0] + i;
780                 T s0 = sptr[0], s1 = sptr[1], s2 = sptr[2], s3 = sptr[3];
781
782                 for( k = 1; k < nz; k++ )
783                 {
784                     sptr = kp[k] + i;
785                     s0 = op(s0, sptr[0]); s1 = op(s1, sptr[1]);
786                     s2 = op(s2, sptr[2]); s3 = op(s3, sptr[3]);
787                 }
788
789                 D[i] = s0; D[i+1] = s1;
790                 D[i+2] = s2; D[i+3] = s3;
791             }
792
793             for( ; i < width; i++ )
794             {
795                 T s0 = kp[0][i];
796                 for( k = 1; k < nz; k++ )
797                     s0 = op(s0, kp[k][i]);
798                 D[i] = s0;
799             }
800         }
801     }
802
803     vector<Point> coords;
804     vector<uchar*> ptrs;
805     VecOp vecOp;
806 };
807
808 /////////////////////////////////// External Interface /////////////////////////////////////
809
810 Ptr<BaseRowFilter> getMorphologyRowFilter(int op, int type, int ksize, int anchor)
811 {
812     int depth = CV_MAT_DEPTH(type);
813     if( anchor < 0 )
814         anchor = ksize/2;
815     CV_Assert( op == MORPH_ERODE || op == MORPH_DILATE );
816     if( op == MORPH_ERODE )
817     {
818         if( depth == CV_8U )
819             return Ptr<BaseRowFilter>(new MorphRowFilter<MinOp<uchar>,
820                 ErodeRowVec8u>(ksize, anchor));
821         if( depth == CV_16U )
822             return Ptr<BaseRowFilter>(new MorphRowFilter<MinOp<ushort>,
823                 ErodeRowVec16u>(ksize, anchor));
824         if( depth == CV_16S )
825             return Ptr<BaseRowFilter>(new MorphRowFilter<MinOp<short>,
826                 ErodeRowVec16s>(ksize, anchor));
827         if( depth == CV_32F )
828             return Ptr<BaseRowFilter>(new MorphRowFilter<MinOp<float>,
829                 ErodeRowVec32f>(ksize, anchor));
830     }
831     else
832     {
833         if( depth == CV_8U )
834             return Ptr<BaseRowFilter>(new MorphRowFilter<MaxOp<uchar>,
835                 DilateRowVec8u>(ksize, anchor));
836         if( depth == CV_16U )
837             return Ptr<BaseRowFilter>(new MorphRowFilter<MaxOp<ushort>,
838                 DilateRowVec16u>(ksize, anchor));
839         if( depth == CV_16S )
840             return Ptr<BaseRowFilter>(new MorphRowFilter<MaxOp<short>,
841                 DilateRowVec16s>(ksize, anchor));
842         if( depth == CV_32F )
843             return Ptr<BaseRowFilter>(new MorphRowFilter<MaxOp<float>,
844                 DilateRowVec32f>(ksize, anchor));
845     }
846
847     CV_Error_( CV_StsNotImplemented, ("Unsupported data type (=%d)", type));
848     return Ptr<BaseRowFilter>(0);
849 }
850
851 Ptr<BaseColumnFilter> getMorphologyColumnFilter(int op, int type, int ksize, int anchor)
852 {
853     int depth = CV_MAT_DEPTH(type);
854     if( anchor < 0 )
855         anchor = ksize/2;
856     CV_Assert( op == MORPH_ERODE || op == MORPH_DILATE );
857     if( op == MORPH_ERODE )
858     {
859         if( depth == CV_8U )
860             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MinOp<uchar>,
861                 ErodeColumnVec8u>(ksize, anchor));
862         if( depth == CV_16U )
863             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MinOp<ushort>,
864                 ErodeColumnVec16u>(ksize, anchor));
865         if( depth == CV_16S )
866             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MinOp<short>,
867                 ErodeColumnVec16s>(ksize, anchor));
868         if( depth == CV_32F )
869             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MinOp<float>,
870                 ErodeColumnVec32f>(ksize, anchor));
871     }
872     else
873     {
874         if( depth == CV_8U )
875             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MaxOp<uchar>,
876                 DilateColumnVec8u>(ksize, anchor));
877         if( depth == CV_16U )
878             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MaxOp<ushort>,
879                 DilateColumnVec16u>(ksize, anchor));
880         if( depth == CV_16S )
881             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MaxOp<short>,
882                 DilateColumnVec16u>(ksize, anchor));
883         if( depth == CV_32F )
884             return Ptr<BaseColumnFilter>(new MorphColumnFilter<MaxOp<float>,
885                 DilateColumnVec32f>(ksize, anchor));
886     }
887
888     CV_Error_( CV_StsNotImplemented, ("Unsupported data type (=%d)", type));
889     return Ptr<BaseColumnFilter>(0);
890 }
891
892
893 Ptr<BaseFilter> getMorphologyFilter(int op, int type, const Mat& kernel, Point anchor)
894 {
895     int depth = CV_MAT_DEPTH(type);
896     anchor = normalizeAnchor(anchor, kernel.size());
897     CV_Assert( op == MORPH_ERODE || op == MORPH_DILATE );
898     if( op == MORPH_ERODE )
899     {
900         if( depth == CV_8U )
901             return Ptr<BaseFilter>(new MorphFilter<MinOp<uchar>, ErodeVec8u>(kernel, anchor));
902         if( depth == CV_16U )
903             return Ptr<BaseFilter>(new MorphFilter<MinOp<ushort>, ErodeVec16u>(kernel, anchor));
904         if( depth == CV_16S )
905             return Ptr<BaseFilter>(new MorphFilter<MinOp<short>, ErodeVec16s>(kernel, anchor));
906         if( depth == CV_32F )
907             return Ptr<BaseFilter>(new MorphFilter<MinOp<float>, ErodeVec32f>(kernel, anchor));
908     }
909     else
910     {
911         if( depth == CV_8U )
912             return Ptr<BaseFilter>(new MorphFilter<MaxOp<uchar>, DilateVec8u>(kernel, anchor));
913         if( depth == CV_16U )
914             return Ptr<BaseFilter>(new MorphFilter<MaxOp<ushort>, DilateVec16u>(kernel, anchor));
915         if( depth == CV_16S )
916             return Ptr<BaseFilter>(new MorphFilter<MaxOp<short>, DilateVec16s>(kernel, anchor));
917         if( depth == CV_32F )
918             return Ptr<BaseFilter>(new MorphFilter<MaxOp<float>, DilateVec32f>(kernel, anchor));
919     }
920
921     CV_Error_( CV_StsNotImplemented, ("Unsupported data type (=%d)", type));
922     return Ptr<BaseFilter>(0);
923 }
924
925
926 Ptr<FilterEngine> createMorphologyFilter( int op, int type, const Mat& kernel,
927          Point anchor, int _rowBorderType, int _columnBorderType,
928          const Scalar& _borderValue )
929 {
930     anchor = normalizeAnchor(anchor, kernel.size());
931
932     Ptr<BaseRowFilter> rowFilter;
933     Ptr<BaseColumnFilter> columnFilter;
934     Ptr<BaseFilter> filter2D;
935
936     if( countNonZero(kernel) == kernel.rows*kernel.cols )
937     {
938         // rectangular structuring element
939         rowFilter = getMorphologyRowFilter(op, type, kernel.cols, anchor.x);
940         columnFilter = getMorphologyColumnFilter(op, type, kernel.rows, anchor.y);
941     }
942     else
943         filter2D = getMorphologyFilter(op, type, kernel, anchor);
944
945     Scalar borderValue = _borderValue;
946     if( (_rowBorderType == BORDER_CONSTANT || _columnBorderType == BORDER_CONSTANT) &&
947         borderValue == morphologyDefaultBorderValue() )
948     {
949         int depth = CV_MAT_TYPE(type);
950         CV_Assert( depth == CV_8U || depth == CV_16U || depth == CV_32F );
951         if( op == MORPH_ERODE )
952             borderValue = Scalar::all( depth == CV_8U ? (double)UCHAR_MAX :
953                 depth == CV_16U ? (double)USHRT_MAX : (double)FLT_MAX );
954         else
955             borderValue = Scalar::all( depth == CV_8U || depth == CV_16U ?
956                 0. : (double)-FLT_MAX );
957     }
958
959     return Ptr<FilterEngine>(new FilterEngine(filter2D, rowFilter, columnFilter,
960         type, type, type, _rowBorderType, _columnBorderType, borderValue ));
961 }
962
963
964 Mat getStructuringElement(int shape, Size ksize, Point anchor)
965 {
966     int i, j;
967     int r = 0, c = 0;
968     double inv_r2 = 0;
969
970     CV_Assert( shape == MORPH_RECT || shape == MORPH_CROSS || shape == MORPH_ELLIPSE );
971
972     anchor = normalizeAnchor(anchor, ksize);
973
974     if( ksize == Size(1,1) )
975         shape = MORPH_RECT;
976
977     if( shape == MORPH_ELLIPSE )
978     {
979         r = ksize.height/2;
980         c = ksize.width/2;
981         inv_r2 = r ? 1./((double)r*r) : 0;
982     }
983
984     Mat elem(ksize, CV_8U);
985
986     for( i = 0; i < ksize.height; i++ )
987     {
988         uchar* ptr = elem.data + i*elem.step;
989         int j1 = 0, j2 = 0;
990
991         if( shape == MORPH_RECT || (shape == MORPH_CROSS && i == anchor.y) )
992             j2 = ksize.width;
993         else if( shape == MORPH_CROSS )
994             j1 = anchor.x, j2 = j1 + 1;
995         else
996         {
997             int dy = i - r;
998             if( std::abs(dy) <= r )
999             {
1000                 int dx = saturate_cast<int>(c*std::sqrt((r*r - dy*dy)*inv_r2));
1001                 j1 = std::max( c - dx, 0 );
1002                 j2 = std::min( c + dx + 1, ksize.width );
1003             }
1004         }
1005
1006         for( j = 0; j < j1; j++ )
1007             ptr[j] = 0;
1008         for( ; j < j2; j++ )
1009             ptr[j] = 1;
1010         for( ; j < ksize.width; j++ )
1011             ptr[j] = 0;
1012     }
1013
1014     return elem;
1015 }
1016
1017 static void morphOp( int op, const Mat& src, Mat& dst, const Mat& _kernel,
1018                      Point anchor, int iterations,
1019                      int borderType, const Scalar& borderValue )
1020 {
1021     Mat kernel;
1022     Size ksize = _kernel.data ? _kernel.size() : Size(3,3);
1023     anchor = normalizeAnchor(anchor, ksize);
1024
1025     CV_Assert( anchor.inside(Rect(0, 0, ksize.width, ksize.height)) );
1026
1027     if( iterations == 0 || _kernel.rows*_kernel.cols == 1 )
1028     {
1029         src.copyTo(dst);
1030         return;
1031     }
1032
1033     dst.create( src.size(), src.type() );
1034
1035     if( !_kernel.data )
1036     {
1037         kernel = getStructuringElement(MORPH_RECT, Size(1+iterations*2,1+iterations*2));
1038         anchor = Point(iterations, iterations);
1039         iterations = 1;
1040     }
1041     else if( iterations > 1 && countNonZero(_kernel) == _kernel.rows*_kernel.cols )
1042     {
1043         anchor = Point(anchor.x*iterations, anchor.y*iterations);
1044         kernel = getStructuringElement(MORPH_RECT,
1045                 Size(ksize.width + iterations*(ksize.width-1),
1046                      ksize.height + iterations*(ksize.height-1)),
1047                 anchor);
1048         iterations = 1;
1049     }
1050     else
1051         kernel = _kernel;
1052
1053     Ptr<FilterEngine> f = createMorphologyFilter(op, src.type(),
1054         kernel, anchor, borderType, borderType, borderValue );
1055
1056     f->apply( src, dst );
1057     for( int i = 1; i < iterations; i++ )
1058         f->apply( dst, dst );
1059 }
1060
1061
1062 void erode( const Mat& src, Mat& dst, const Mat& kernel,
1063             Point anchor, int iterations,
1064             int borderType, const Scalar& borderValue )
1065 {
1066     morphOp( MORPH_ERODE, src, dst, kernel, anchor, iterations, borderType, borderValue );
1067 }
1068
1069
1070 void dilate( const Mat& src, Mat& dst, const Mat& kernel,
1071              Point anchor, int iterations,
1072              int borderType, const Scalar& borderValue )
1073 {
1074     morphOp( MORPH_DILATE, src, dst, kernel, anchor, iterations, borderType, borderValue );
1075 }
1076
1077
1078 void morphologyEx( const Mat& src, Mat& dst, int op, const Mat& kernel,
1079                    Point anchor, int iterations, int borderType,
1080                    const Scalar& borderValue )
1081 {
1082     Mat temp;
1083     switch( op )
1084     {
1085     case MORPH_OPEN:
1086         erode( src, dst, kernel, anchor, iterations, borderType, borderValue );
1087         dilate( dst, dst, kernel, anchor, iterations, borderType, borderValue );
1088         break;
1089     case CV_MOP_CLOSE:
1090         dilate( src, dst, kernel, anchor, iterations, borderType, borderValue );
1091         erode( dst, dst, kernel, anchor, iterations, borderType, borderValue );
1092         break;
1093     case CV_MOP_GRADIENT:
1094         erode( src, temp, kernel, anchor, iterations, borderType, borderValue );
1095         dilate( src, dst, kernel, anchor, iterations, borderType, borderValue );
1096         dst -= temp;
1097         break;
1098     case CV_MOP_TOPHAT:
1099         if( src.data != dst.data )
1100             temp = dst;
1101         erode( src, temp, kernel, anchor, iterations, borderType, borderValue );
1102         dilate( temp, temp, kernel, anchor, iterations, borderType, borderValue );
1103         dst = src - temp;
1104         break;
1105     case CV_MOP_BLACKHAT:
1106         if( src.data != dst.data )
1107             temp = dst;
1108         dilate( src, temp, kernel, anchor, iterations, borderType, borderValue );
1109         erode( temp, temp, kernel, anchor, iterations, borderType, borderValue );
1110         dst = temp - src;
1111         break;
1112     default:
1113         CV_Error( CV_StsBadArg, "unknown morphological operation" );
1114     }
1115 }
1116
1117 }
1118
1119 CV_IMPL IplConvKernel *
1120 cvCreateStructuringElementEx( int cols, int rows,
1121                               int anchorX, int anchorY,
1122                               int shape, int *values )
1123 {
1124     cv::Size ksize = cv::Size(cols, rows);
1125     cv::Point anchor = cv::Point(anchorX, anchorY);
1126     CV_Assert( cols > 0 && rows > 0 && anchor.inside(cv::Rect(0,0,cols,rows)) &&
1127         (shape != CV_SHAPE_CUSTOM || values != 0));
1128
1129     int i, size = rows * cols;
1130     int element_size = sizeof(IplConvKernel) + size*sizeof(int);
1131     IplConvKernel *element = (IplConvKernel*)cvAlloc(element_size + 32);
1132
1133     element->nCols = cols;
1134     element->nRows = rows;
1135     element->anchorX = anchorX;
1136     element->anchorY = anchorY;
1137     element->nShiftR = shape < CV_SHAPE_ELLIPSE ? shape : CV_SHAPE_CUSTOM;
1138     element->values = (int*)(element + 1);
1139
1140     if( shape == CV_SHAPE_CUSTOM )
1141     {
1142         for( i = 0; i < size; i++ )
1143             element->values[i] = values[i];
1144     }
1145     else
1146     {
1147         cv::Mat elem = cv::getStructuringElement(shape, ksize, anchor);
1148         for( i = 0; i < size; i++ )
1149             element->values[i] = elem.data[i];
1150     }
1151
1152     return element;
1153 }
1154
1155
1156 CV_IMPL void
1157 cvReleaseStructuringElement( IplConvKernel ** element )
1158 {
1159     if( !element )
1160         CV_Error( CV_StsNullPtr, "" );
1161     cvFree( element );
1162 }
1163
1164
1165 static void convertConvKernel( const IplConvKernel* src, cv::Mat& dst, cv::Point& anchor )
1166 {
1167     if(!src)
1168     {
1169         anchor = cv::Point(1,1);
1170         dst.release();
1171         return;
1172     }
1173     anchor = cv::Point(src->anchorX, src->anchorY);
1174     dst.create(src->nRows, src->nCols, CV_8U);
1175
1176     int i, size = src->nRows*src->nCols;
1177     for( i = 0; i < size; i++ )
1178         dst.data[i] = (uchar)src->values[i];
1179 }
1180
1181
1182 CV_IMPL void
1183 cvErode( const CvArr* srcarr, CvArr* dstarr, IplConvKernel* element, int iterations )
1184 {
1185     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), kernel;
1186     CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
1187     cv::Point anchor;
1188     convertConvKernel( element, kernel, anchor );
1189     cv::erode( src, dst, kernel, anchor, iterations, cv::BORDER_REPLICATE );
1190 }
1191
1192
1193 CV_IMPL void
1194 cvDilate( const CvArr* srcarr, CvArr* dstarr, IplConvKernel* element, int iterations )
1195 {
1196     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), kernel;
1197     CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
1198     cv::Point anchor;
1199     convertConvKernel( element, kernel, anchor );
1200     cv::dilate( src, dst, kernel, anchor, iterations, cv::BORDER_REPLICATE );
1201 }
1202
1203
1204 CV_IMPL void
1205 cvMorphologyEx( const void* srcarr, void* dstarr, void*,
1206                 IplConvKernel* element, int op, int iterations )
1207 {
1208     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), kernel;
1209     CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
1210     cv::Point anchor;
1211         IplConvKernel* temp_element = NULL;
1212     if (!element)
1213     {
1214         temp_element = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT);
1215     } else {
1216         temp_element = element;
1217     }
1218     convertConvKernel( temp_element, kernel, anchor );
1219     if (!element)
1220     {
1221         cvReleaseStructuringElement(&temp_element);
1222     }
1223     cv::morphologyEx( src, dst, op, kernel, anchor, iterations, cv::BORDER_REPLICATE );
1224 }
1225
1226 /* End of file. */