Add OpenCV source code
[platform/upstream/opencv.git] / modules / core / src / copy.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-2011, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 //   * Redistribution's of source code must retain the above copyright notice,
21 //     this list of conditions and the following disclaimer.
22 //
23 //   * Redistribution's in binary form must reproduce the above copyright notice,
24 //     this list of conditions and the following disclaimer in the documentation
25 //     and/or other materials provided with the distribution.
26 //
27 //   * The name of the copyright holders may not be used to endorse or promote products
28 //     derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 /* ////////////////////////////////////////////////////////////////////
44 //
45 //  Mat basic operations: Copy, Set
46 //
47 // */
48
49 #include "precomp.hpp"
50
51 namespace cv
52 {
53
54 template<typename T> static void
55 copyMask_(const uchar* _src, size_t sstep, const uchar* mask, size_t mstep, uchar* _dst, size_t dstep, Size size)
56 {
57     for( ; size.height--; mask += mstep, _src += sstep, _dst += dstep )
58     {
59         const T* src = (const T*)_src;
60         T* dst = (T*)_dst;
61         int x = 0;
62          #if CV_ENABLE_UNROLLED
63         for( ; x <= size.width - 4; x += 4 )
64         {
65             if( mask[x] )
66                 dst[x] = src[x];
67             if( mask[x+1] )
68                 dst[x+1] = src[x+1];
69             if( mask[x+2] )
70                 dst[x+2] = src[x+2];
71             if( mask[x+3] )
72                 dst[x+3] = src[x+3];
73         }
74         #endif
75         for( ; x < size.width; x++ )
76             if( mask[x] )
77                 dst[x] = src[x];
78     }
79 }
80
81 template<> void
82 copyMask_<uchar>(const uchar* _src, size_t sstep, const uchar* mask, size_t mstep, uchar* _dst, size_t dstep, Size size)
83 {
84     for( ; size.height--; mask += mstep, _src += sstep, _dst += dstep )
85     {
86         const uchar* src = (const uchar*)_src;
87         uchar* dst = (uchar*)_dst;
88         int x = 0;
89         #if CV_SSE4_2
90         if(USE_SSE4_2)//
91         {
92             __m128i zero = _mm_setzero_si128 ();
93
94              for( ; x <= size.width - 16; x += 16 )
95              {
96                  const __m128i rSrc = _mm_lddqu_si128((const __m128i*)(src+x));
97                  __m128i _mask = _mm_lddqu_si128((const __m128i*)(mask+x));
98                  __m128i rDst = _mm_lddqu_si128((__m128i*)(dst+x));
99                  __m128i _negMask = _mm_cmpeq_epi8(_mask, zero);
100                  rDst = _mm_blendv_epi8(rSrc, rDst, _negMask);
101                  _mm_storeu_si128((__m128i*)(dst + x), rDst);
102              }
103         }
104         #endif
105         for( ; x < size.width; x++ )
106             if( mask[x] )
107                 dst[x] = src[x];
108     }
109 }
110
111 template<> void
112 copyMask_<ushort>(const uchar* _src, size_t sstep, const uchar* mask, size_t mstep, uchar* _dst, size_t dstep, Size size)
113 {
114     for( ; size.height--; mask += mstep, _src += sstep, _dst += dstep )
115     {
116         const ushort* src = (const ushort*)_src;
117         ushort* dst = (ushort*)_dst;
118         int x = 0;
119         #if CV_SSE4_2
120         if(USE_SSE4_2)//
121         {
122             __m128i zero = _mm_setzero_si128 ();
123             for( ; x <= size.width - 8; x += 8 )
124             {
125                  const __m128i rSrc =_mm_lddqu_si128((const __m128i*)(src+x));
126                  __m128i _mask = _mm_loadl_epi64((const __m128i*)(mask+x));
127                  _mask = _mm_unpacklo_epi8(_mask, _mask);
128                  __m128i rDst = _mm_lddqu_si128((const __m128i*)(dst+x));
129                  __m128i _negMask = _mm_cmpeq_epi8(_mask, zero);
130                  rDst = _mm_blendv_epi8(rSrc, rDst, _negMask);
131                  _mm_storeu_si128((__m128i*)(dst + x), rDst);
132              }
133         }
134         #endif
135         for( ; x < size.width; x++ )
136             if( mask[x] )
137                 dst[x] = src[x];
138     }
139 }
140
141 static void
142 copyMaskGeneric(const uchar* _src, size_t sstep, const uchar* mask, size_t mstep, uchar* _dst, size_t dstep, Size size, void* _esz)
143 {
144     size_t k, esz = *(size_t*)_esz;
145     for( ; size.height--; mask += mstep, _src += sstep, _dst += dstep )
146     {
147         const uchar* src = _src;
148         uchar* dst = _dst;
149         int x = 0;
150         for( ; x < size.width; x++, src += esz, dst += esz )
151         {
152             if( !mask[x] )
153                 continue;
154             for( k = 0; k < esz; k++ )
155                 dst[k] = src[k];
156         }
157     }
158 }
159
160
161 #define DEF_COPY_MASK(suffix, type) \
162 static void copyMask##suffix(const uchar* src, size_t sstep, const uchar* mask, size_t mstep, \
163                              uchar* dst, size_t dstep, Size size, void*) \
164 { \
165     copyMask_<type>(src, sstep, mask, mstep, dst, dstep, size); \
166 }
167
168
169 DEF_COPY_MASK(8u, uchar)
170 DEF_COPY_MASK(16u, ushort)
171 DEF_COPY_MASK(8uC3, Vec3b)
172 DEF_COPY_MASK(32s, int)
173 DEF_COPY_MASK(16uC3, Vec3s)
174 DEF_COPY_MASK(32sC2, Vec2i)
175 DEF_COPY_MASK(32sC3, Vec3i)
176 DEF_COPY_MASK(32sC4, Vec4i)
177 DEF_COPY_MASK(32sC6, Vec6i)
178 DEF_COPY_MASK(32sC8, Vec8i)
179
180 BinaryFunc copyMaskTab[] =
181 {
182     0,
183     copyMask8u,
184     copyMask16u,
185     copyMask8uC3,
186     copyMask32s,
187     0,
188     copyMask16uC3,
189     0,
190     copyMask32sC2,
191     0, 0, 0,
192     copyMask32sC3,
193     0, 0, 0,
194     copyMask32sC4,
195     0, 0, 0, 0, 0, 0, 0,
196     copyMask32sC6,
197     0, 0, 0, 0, 0, 0, 0,
198     copyMask32sC8
199 };
200
201 BinaryFunc getCopyMaskFunc(size_t esz)
202 {
203     return esz <= 32 && copyMaskTab[esz] ? copyMaskTab[esz] : copyMaskGeneric;
204 }
205
206 /* dst = src */
207 void Mat::copyTo( OutputArray _dst ) const
208 {
209     int dtype = _dst.type();
210     if( _dst.fixedType() && dtype != type() )
211     {
212         CV_Assert( channels() == CV_MAT_CN(dtype) );
213         convertTo( _dst, dtype );
214         return;
215     }
216
217     if( empty() )
218     {
219         _dst.release();
220         return;
221     }
222
223     if( dims <= 2 )
224     {
225         _dst.create( rows, cols, type() );
226         Mat dst = _dst.getMat();
227         if( data == dst.data )
228             return;
229
230         if( rows > 0 && cols > 0 )
231         {
232             const uchar* sptr = data;
233             uchar* dptr = dst.data;
234
235             Size sz = getContinuousSize(*this, dst);
236             size_t len = sz.width*elemSize();
237
238             for( ; sz.height--; sptr += step, dptr += dst.step )
239                 memcpy( dptr, sptr, len );
240         }
241         return;
242     }
243
244     _dst.create( dims, size, type() );
245     Mat dst = _dst.getMat();
246     if( data == dst.data )
247         return;
248
249     if( total() != 0 )
250     {
251         const Mat* arrays[] = { this, &dst };
252         uchar* ptrs[2];
253         NAryMatIterator it(arrays, ptrs, 2);
254         size_t sz = it.size*elemSize();
255
256         for( size_t i = 0; i < it.nplanes; i++, ++it )
257             memcpy(ptrs[1], ptrs[0], sz);
258     }
259 }
260
261 void Mat::copyTo( OutputArray _dst, InputArray _mask ) const
262 {
263     Mat mask = _mask.getMat();
264     if( !mask.data )
265     {
266         copyTo(_dst);
267         return;
268     }
269
270     int cn = channels(), mcn = mask.channels();
271     CV_Assert( mask.depth() == CV_8U && (mcn == 1 || mcn == cn) );
272     bool colorMask = mcn > 1;
273
274     size_t esz = colorMask ? elemSize1() : elemSize();
275     BinaryFunc copymask = getCopyMaskFunc(esz);
276
277     uchar* data0 = _dst.getMat().data;
278     _dst.create( dims, size, type() );
279     Mat dst = _dst.getMat();
280
281     if( dst.data != data0 ) // do not leave dst uninitialized
282         dst = Scalar(0);
283
284     if( dims <= 2 )
285     {
286         CV_Assert( size() == mask.size() );
287         Size sz = getContinuousSize(*this, dst, mask, mcn);
288         copymask(data, step, mask.data, mask.step, dst.data, dst.step, sz, &esz);
289         return;
290     }
291
292     const Mat* arrays[] = { this, &dst, &mask, 0 };
293     uchar* ptrs[3];
294     NAryMatIterator it(arrays, ptrs);
295     Size sz((int)(it.size*mcn), 1);
296
297     for( size_t i = 0; i < it.nplanes; i++, ++it )
298         copymask(ptrs[0], 0, ptrs[2], 0, ptrs[1], 0, sz, &esz);
299 }
300
301 Mat& Mat::operator = (const Scalar& s)
302 {
303     const Mat* arrays[] = { this };
304     uchar* dptr;
305     NAryMatIterator it(arrays, &dptr, 1);
306     size_t elsize = it.size*elemSize();
307     const int64* is = (const int64*)&s.val[0];
308
309     if( is[0] == 0 && is[1] == 0 && is[2] == 0 && is[3] == 0 )
310     {
311         for( size_t i = 0; i < it.nplanes; i++, ++it )
312             memset( dptr, 0, elsize );
313     }
314     else
315     {
316         if( it.nplanes > 0 )
317         {
318             double scalar[12];
319             scalarToRawData(s, scalar, type(), 12);
320             size_t blockSize = 12*elemSize1();
321
322             for( size_t j = 0; j < elsize; j += blockSize )
323             {
324                 size_t sz = MIN(blockSize, elsize - j);
325                 memcpy( dptr + j, scalar, sz );
326             }
327         }
328
329         for( size_t i = 1; i < it.nplanes; i++ )
330         {
331             ++it;
332             memcpy( dptr, data, elsize );
333         }
334     }
335     return *this;
336 }
337
338
339 Mat& Mat::setTo(InputArray _value, InputArray _mask)
340 {
341     if( !data )
342         return *this;
343
344     Mat value = _value.getMat(), mask = _mask.getMat();
345
346     CV_Assert( checkScalar(value, type(), _value.kind(), _InputArray::MAT ));
347     CV_Assert( mask.empty() || mask.type() == CV_8U );
348
349     size_t esz = elemSize();
350     BinaryFunc copymask = getCopyMaskFunc(esz);
351
352     const Mat* arrays[] = { this, !mask.empty() ? &mask : 0, 0 };
353     uchar* ptrs[2]={0,0};
354     NAryMatIterator it(arrays, ptrs);
355     int totalsz = (int)it.size, blockSize0 = std::min(totalsz, (int)((BLOCK_SIZE + esz-1)/esz));
356     AutoBuffer<uchar> _scbuf(blockSize0*esz + 32);
357     uchar* scbuf = alignPtr((uchar*)_scbuf, (int)sizeof(double));
358     convertAndUnrollScalar( value, type(), scbuf, blockSize0 );
359
360     for( size_t i = 0; i < it.nplanes; i++, ++it )
361     {
362         for( int j = 0; j < totalsz; j += blockSize0 )
363         {
364             Size sz(std::min(blockSize0, totalsz - j), 1);
365             size_t blockSize = sz.width*esz;
366             if( ptrs[1] )
367             {
368                 copymask(scbuf, 0, ptrs[1], 0, ptrs[0], 0, sz, &esz);
369                 ptrs[1] += sz.width;
370             }
371             else
372                 memcpy(ptrs[0], scbuf, blockSize);
373             ptrs[0] += blockSize;
374         }
375     }
376     return *this;
377 }
378
379
380 static void
381 flipHoriz( const uchar* src, size_t sstep, uchar* dst, size_t dstep, Size size, size_t esz )
382 {
383     int i, j, limit = (int)(((size.width + 1)/2)*esz);
384     AutoBuffer<int> _tab(size.width*esz);
385     int* tab = _tab;
386
387     for( i = 0; i < size.width; i++ )
388         for( size_t k = 0; k < esz; k++ )
389             tab[i*esz + k] = (int)((size.width - i - 1)*esz + k);
390
391     for( ; size.height--; src += sstep, dst += dstep )
392     {
393         for( i = 0; i < limit; i++ )
394         {
395             j = tab[i];
396             uchar t0 = src[i], t1 = src[j];
397             dst[i] = t1; dst[j] = t0;
398         }
399     }
400 }
401
402 static void
403 flipVert( const uchar* src0, size_t sstep, uchar* dst0, size_t dstep, Size size, size_t esz )
404 {
405     const uchar* src1 = src0 + (size.height - 1)*sstep;
406     uchar* dst1 = dst0 + (size.height - 1)*dstep;
407     size.width *= (int)esz;
408
409     for( int y = 0; y < (size.height + 1)/2; y++, src0 += sstep, src1 -= sstep,
410                                                   dst0 += dstep, dst1 -= dstep )
411     {
412         int i = 0;
413         if( ((size_t)src0|(size_t)dst0|(size_t)src1|(size_t)dst1) % sizeof(int) == 0 )
414         {
415             for( ; i <= size.width - 16; i += 16 )
416             {
417                 int t0 = ((int*)(src0 + i))[0];
418                 int t1 = ((int*)(src1 + i))[0];
419
420                 ((int*)(dst0 + i))[0] = t1;
421                 ((int*)(dst1 + i))[0] = t0;
422
423                 t0 = ((int*)(src0 + i))[1];
424                 t1 = ((int*)(src1 + i))[1];
425
426                 ((int*)(dst0 + i))[1] = t1;
427                 ((int*)(dst1 + i))[1] = t0;
428
429                 t0 = ((int*)(src0 + i))[2];
430                 t1 = ((int*)(src1 + i))[2];
431
432                 ((int*)(dst0 + i))[2] = t1;
433                 ((int*)(dst1 + i))[2] = t0;
434
435                 t0 = ((int*)(src0 + i))[3];
436                 t1 = ((int*)(src1 + i))[3];
437
438                 ((int*)(dst0 + i))[3] = t1;
439                 ((int*)(dst1 + i))[3] = t0;
440             }
441
442             for( ; i <= size.width - 4; i += 4 )
443             {
444                 int t0 = ((int*)(src0 + i))[0];
445                 int t1 = ((int*)(src1 + i))[0];
446
447                 ((int*)(dst0 + i))[0] = t1;
448                 ((int*)(dst1 + i))[0] = t0;
449             }
450         }
451
452         for( ; i < size.width; i++ )
453         {
454             uchar t0 = src0[i];
455             uchar t1 = src1[i];
456
457             dst0[i] = t1;
458             dst1[i] = t0;
459         }
460     }
461 }
462
463 void flip( InputArray _src, OutputArray _dst, int flip_mode )
464 {
465     Mat src = _src.getMat();
466
467     CV_Assert( src.dims <= 2 );
468     _dst.create( src.size(), src.type() );
469     Mat dst = _dst.getMat();
470     size_t esz = src.elemSize();
471
472     if( flip_mode <= 0 )
473         flipVert( src.data, src.step, dst.data, dst.step, src.size(), esz );
474     else
475         flipHoriz( src.data, src.step, dst.data, dst.step, src.size(), esz );
476
477     if( flip_mode < 0 )
478         flipHoriz( dst.data, dst.step, dst.data, dst.step, dst.size(), esz );
479 }
480
481
482 void repeat(InputArray _src, int ny, int nx, OutputArray _dst)
483 {
484     Mat src = _src.getMat();
485     CV_Assert( src.dims <= 2 );
486     CV_Assert( ny > 0 && nx > 0 );
487
488     _dst.create(src.rows*ny, src.cols*nx, src.type());
489     Mat dst = _dst.getMat();
490     Size ssize = src.size(), dsize = dst.size();
491     int esz = (int)src.elemSize();
492     int x, y;
493     ssize.width *= esz; dsize.width *= esz;
494
495     for( y = 0; y < ssize.height; y++ )
496     {
497         for( x = 0; x < dsize.width; x += ssize.width )
498             memcpy( dst.data + y*dst.step + x, src.data + y*src.step, ssize.width );
499     }
500
501     for( ; y < dsize.height; y++ )
502         memcpy( dst.data + y*dst.step, dst.data + (y - ssize.height)*dst.step, dsize.width );
503 }
504
505 Mat repeat(const Mat& src, int ny, int nx)
506 {
507     if( nx == 1 && ny == 1 )
508         return src;
509     Mat dst;
510     repeat(src, ny, nx, dst);
511     return dst;
512 }
513
514 }
515
516 /* dst = src */
517 CV_IMPL void
518 cvCopy( const void* srcarr, void* dstarr, const void* maskarr )
519 {
520     if( CV_IS_SPARSE_MAT(srcarr) && CV_IS_SPARSE_MAT(dstarr))
521     {
522         CV_Assert( maskarr == 0 );
523         CvSparseMat* src1 = (CvSparseMat*)srcarr;
524         CvSparseMat* dst1 = (CvSparseMat*)dstarr;
525         CvSparseMatIterator iterator;
526         CvSparseNode* node;
527
528         dst1->dims = src1->dims;
529         memcpy( dst1->size, src1->size, src1->dims*sizeof(src1->size[0]));
530         dst1->valoffset = src1->valoffset;
531         dst1->idxoffset = src1->idxoffset;
532         cvClearSet( dst1->heap );
533
534         if( src1->heap->active_count >= dst1->hashsize*CV_SPARSE_HASH_RATIO )
535         {
536             cvFree( &dst1->hashtable );
537             dst1->hashsize = src1->hashsize;
538             dst1->hashtable =
539                 (void**)cvAlloc( dst1->hashsize*sizeof(dst1->hashtable[0]));
540         }
541
542         memset( dst1->hashtable, 0, dst1->hashsize*sizeof(dst1->hashtable[0]));
543
544         for( node = cvInitSparseMatIterator( src1, &iterator );
545              node != 0; node = cvGetNextSparseNode( &iterator ))
546         {
547             CvSparseNode* node_copy = (CvSparseNode*)cvSetNew( dst1->heap );
548             int tabidx = node->hashval & (dst1->hashsize - 1);
549             memcpy( node_copy, node, dst1->heap->elem_size );
550             node_copy->next = (CvSparseNode*)dst1->hashtable[tabidx];
551             dst1->hashtable[tabidx] = node_copy;
552         }
553         return;
554     }
555     cv::Mat src = cv::cvarrToMat(srcarr, false, true, 1), dst = cv::cvarrToMat(dstarr, false, true, 1);
556     CV_Assert( src.depth() == dst.depth() && src.size == dst.size );
557
558     int coi1 = 0, coi2 = 0;
559     if( CV_IS_IMAGE(srcarr) )
560         coi1 = cvGetImageCOI((const IplImage*)srcarr);
561     if( CV_IS_IMAGE(dstarr) )
562         coi2 = cvGetImageCOI((const IplImage*)dstarr);
563
564     if( coi1 || coi2 )
565     {
566         CV_Assert( (coi1 != 0 || src.channels() == 1) &&
567             (coi2 != 0 || dst.channels() == 1) );
568
569         int pair[] = { std::max(coi1-1, 0), std::max(coi2-1, 0) };
570         cv::mixChannels( &src, 1, &dst, 1, pair, 1 );
571         return;
572     }
573     else
574         CV_Assert( src.channels() == dst.channels() );
575
576     if( !maskarr )
577         src.copyTo(dst);
578     else
579         src.copyTo(dst, cv::cvarrToMat(maskarr));
580 }
581
582 CV_IMPL void
583 cvSet( void* arr, CvScalar value, const void* maskarr )
584 {
585     cv::Mat m = cv::cvarrToMat(arr);
586     if( !maskarr )
587         m = value;
588     else
589         m.setTo(cv::Scalar(value), cv::cvarrToMat(maskarr));
590 }
591
592 CV_IMPL void
593 cvSetZero( CvArr* arr )
594 {
595     if( CV_IS_SPARSE_MAT(arr) )
596     {
597         CvSparseMat* mat1 = (CvSparseMat*)arr;
598         cvClearSet( mat1->heap );
599         if( mat1->hashtable )
600             memset( mat1->hashtable, 0, mat1->hashsize*sizeof(mat1->hashtable[0]));
601         return;
602     }
603     cv::Mat m = cv::cvarrToMat(arr);
604     m = cv::Scalar(0);
605 }
606
607 CV_IMPL void
608 cvFlip( const CvArr* srcarr, CvArr* dstarr, int flip_mode )
609 {
610     cv::Mat src = cv::cvarrToMat(srcarr);
611     cv::Mat dst;
612
613     if (!dstarr)
614       dst = src;
615     else
616       dst = cv::cvarrToMat(dstarr);
617
618     CV_Assert( src.type() == dst.type() && src.size() == dst.size() );
619     cv::flip( src, dst, flip_mode );
620 }
621
622 CV_IMPL void
623 cvRepeat( const CvArr* srcarr, CvArr* dstarr )
624 {
625     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
626     CV_Assert( src.type() == dst.type() &&
627         dst.rows % src.rows == 0 && dst.cols % src.cols == 0 );
628     cv::repeat(src, dst.rows/src.rows, dst.cols/src.cols, dst);
629 }
630
631 /* End of file. */