开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
processingMMX.inl
资源名称：estereo2.zip　[点击查看]
上传用户：fengshi120
上传日期：2014-07-17
资源大小：6155k
文件大小：54k
源码类别：

3D图形编程

开发平台：

C/C++

processingMMX.inl：源码内容
  1. /*************************************************************************** 
  2. *
  3. * Copyright 2000 by David Demirdjian.   All rights reserved. 
  4. *  
  5. * Developed  by David Demirdjian
  6. *  
  7. * Permission to use, copy, or modify this software and  its documentation 
  8. * for  educational  and  research purposes only and without fee  is hereby 
  9. * granted, provided  that this copyright notice and the original authors's 
  10. * names appear  on all copies and supporting documentation.  If individual 
  11. * files are  separated from  this  distribution directory  structure, this 
  12. * copyright notice must be included.  For any other uses of this software, 
  13. * in original or  modified form, including but not limited to distribution 
  14. * in whole or in  part, specific  prior permission  must be  obtained from 
  15. * MIT.  These programs shall not  be  used, rewritten, or  adapted as  the 
  16. * basis  of  a  commercial  software  or  hardware product  without  first 
  17. * obtaining appropriate  licenses from David Demirdjian.  The author makes 
  18. * no representations about the suitability of this software for any purpose.  
  19. * It is provided "as is" without express or implied warranty. 
  20. *  
  21. **************************************************************************/
  22. #include "stereoMatching.h"
  23. #include "processingMMX.h"
  25. // ************************************************************
  26. // ************************************************************
  27. // *** List of functions (MMX) for image processing
  28. // ************************************************************
  29. // ************************************************************
  31. // shrink images by a factor 'fact'. eg if fact = 2, out will be twice as small as src
  32. inline void shrinkImages(uchar* dst, const uchar* src, int width, int height, int fact)
  33. {
  34. int width_f = width/fact;
  35. int siz = width*height/(fact*fact);
  36. int src_idx = 0;
  37. if (fact>0) {
  38. for (int i=0,j=0; i<siz && src_idx<width*height; ++i,++j,++dst,src+=fact,src_idx+=fact) {
  39. *dst = *src;
  40. if (j==width_f-1) {
  41. src+=((fact-1)*width);
  42. src_idx+=((fact-1)*width);
  43. j=0;
  44. }
  45. }
  46. }
  47. }
  49. // translate image of 'tx' pixels to the right
  50. // (or left if tx<0)
  51. void translateImage(int tx, uchar* data, int siz)
  52. {
  53. if (tx==0) return; 
  54. if (tx>0) {
  55. // dest. must be after src... to avoid overwriting data
  56. data += (siz-tx);
  57. uchar* dataDst = data+tx;
  58. for (int i=0; i<siz-tx; ++i,--dataDst,--data) {
  59. *dataDst = *data;
  60. }
  61. } else { // tx<0
  62. data -= tx;
  63. uchar* dataDst = data+tx;
  64. for (int i=0; i<siz-tx; ++i,++dataDst,++data) {
  65. *dataDst = *data;
  66. }
  67. }
  68. }
  70. void normalizeImages(uchar* data1, uchar* data2, uchar* data3, int siz)
  71. {
  72. float a1 = pixelMean(data1,siz);
  73. float a2 = pixelMean(data2,siz);
  74. float a3 = pixelMean(data3,siz);
  76. float minI = __min(a1, __min(a2,a3));
  78. if (a2==minI) {
  79. multiply(data1, a2/a1, siz);
  80. multiply(data3, a2/a3, siz);
  81. } else if (a1==minI) {
  82. multiply(data2, a1/a2, siz);
  83. multiply(data3, a1/a3, siz);
  84. } else {
  85. multiply(data2, a3/a2, siz);
  86. multiply(data1, a3/a1, siz);
  87. }
  88. }
  90. void normalizeImages(const uchar* data1, const uchar* data2, const uchar* data3, 
  91.  uchar* out1, uchar* out2, uchar* out3, int siz)
  92. {
  93. float a1 = pixelMean(data1,siz);
  94. float a2 = pixelMean(data2,siz);
  95. float a3 = pixelMean(data3,siz);
  97. float minI = __min(a1, __min(a2,a3));
  99. if (a2==minI) {
  100. multiply(data1, out1, a2/a1, siz);
  101. multiply(data3, out3, a2/a3, siz);
  102. copyMMX(out2, data2, siz);
  103. } else if (a1==minI) {
  104. multiply(data2, out2, a1/a2, siz);
  105. multiply(data3, out3, a1/a3, siz);
  106. copyMMX(out1, data1, siz);
  107. } else {
  108. multiply(data2, out2, a3/a2, siz);
  109. multiply(data1, out1, a3/a1, siz);
  110. copyMMX(out3, data3, siz);
  111. }
  112. }
  114. void normalizeImages(uchar* data1, uchar* data2, int siz)
  115. {
  116. float a1 = pixelMean(data1,siz);
  117. float a2 = pixelMean(data2,siz);
  119. // normalize the image which average intensity is the highest
  120. if (a1>a2) 
  121. multiply(data1, a2/a1, siz);
  122. else 
  123. multiply(data2, a1/a2, siz);
  124. }
  126. void normalizeImages(const uchar* data1, const uchar* data2, 
  127.  uchar* out1, uchar* out2, int siz)
  128. {
  129. float a1 = pixelMean(data1,siz);
  130. float a2 = pixelMean(data2,siz);
  132. // normalize the image which average intensity is the highest
  133. if (a1>a2) {
  134. multiply(data1, out1, a2/a1, siz);
  135. copyMMX(out2, data2, siz);
  136. } else {
  137. multiply(data2, out2, a1/a2, siz);
  138. copyMMX(out1, data1, siz);
  139. }
  140. }
  142. //  ImgSub2: D = saturation0(|S1 - S2| + |S1 - S3|)
  143. // TODO? divide the result by 2 (shift)
  144. inline int ImgSubandAdd(const unsigned char *Src1, const unsigned char *Src2, 
  145.  const unsigned char *Src3, unsigned char *Dest, int l)
  146. {
  148. if (l < 8) return 0;              // image size must be at least 8 bytes 
  150.   __asm 
  151.   {
  152.         mov eax, Src1     
  153.         mov ebx, Src2
  154. mov edx, Src3
  155.         mov edi, Dest    
  156.         mov ecx, l   
  157.         shr ecx, 3
  159. align 16
  160. inner_loop:
  161. movq mm1,[eax] // mm1=src1
  163. movq mm2,[ebx] // mm2=src2
  165. movq mm4,mm1 // mm4=mm1
  167. psubusb mm4,mm2 // mm4 = src1 - src2
  169. movq mm3,[edx] // mm3=src3
  170. psubusb mm2,mm1 // mm2 = src2 - src1
  171.         
  172. movq mm5,mm1 // mm5=src1
  173. por mm2,mm4 // mm2=|src1-src2|
  175.         psubusb mm5,mm3 // mm4=src1-src3
  177.         psubusb mm3,mm1   // mm3=src3-src1
  179. por mm3,mm5 // mm3=|src1-src3|
  181. paddusb mm2,mm3 // mm2 = |src1-src2|+|src1-src3|
  183.         movq    [edi], mm2  
  184.         add eax,8         
  185.         add ebx,8     
  186.         add edx,8     
  187.         add edi,8
  188.         dec ecx      
  189.         jnz inner_loop    
  190.         emms   
  191.   }
  193.   return 1;
  194. }
  196. //  ImgSub2: D = saturation0(|S1 - S2|)
  197. // TODO? divide the result by 2 (shift)
  198. inline int ImgSubandAdd(const unsigned char *Src1, const unsigned char *Src2, 
  199.  const unsigned char *Dest, int l)
  200. {
  202. if (l < 8) return 0;              // image size must be at least 8 bytes 
  204.   __asm 
  205.   {
  206.         mov eax, Src1     
  207.         mov ebx, Src2
  208.         mov edi, Dest    
  209.         mov ecx, l   
  210.         shr ecx, 3
  212. align 16
  213. inner_loop:
  214. movq mm1,[eax] // mm1=src1
  215. movq mm2,[ebx] // mm2=src2
  217. movq mm4,mm1 // mm4=mm1
  218. psubusb mm4,mm2 // mm4 = src1 - src2
  220. psubusb mm2,mm1 // mm2 = src2 - src1  
  221. por mm2,mm4 // mm2=|src1-src2|
  223.         movq    [edi], mm2  
  224.         add eax,8         
  225.         add ebx,8     
  226.         add edi,8
  227.         dec ecx      
  228.         jnz inner_loop    
  229.         emms   
  230.   }
  232.   return 1;
  233. }
  240. #define _ABS_DIFF_TRI(Z) __asm 
  241. { 
  242. __asm movq mm4,mm1 /* mm4=mm1 */ 
  243. __asm add ebx, width  
  244. __asm add edi, imageSize 
  245. __asm por mm3,mm7 /* here mm2=new src2 mm3=new src3 */ 
  247. __asm movq mm7, mm0 
  248. __asm psubusb mm4,mm2 /* mm4 = src1 - src2 */ 
  250. __asm psubusb mm2,mm1 /* mm2 = src2 - src1 */ 
  251. __asm psllq mm7,Z
  253. __asm movq mm5,mm1 /* mm5=src1 */ 
  254. __asm por mm4,mm2 /* mm2=|src1-src2| */ 
  256. __asm movq mm2,[ebx] /* mm2= src2 + 'width' = new src2*/ 
  257. __asm psubusb mm5,mm3 /* mm5=src1-src3*/ 
  259. __asm movq mm6,mm3 /* mm6=src3*/ 
  260.     __asm   psubusb mm6,mm1   /* mm3=src3-src1*/ 
  262. __asm por mm6,mm5 /* mm6=|src1-src3|*/ 
  263. __asm paddusb mm4,mm6 /* mm4 = |src1-src2|+|src1-src3|*/ 
  265. __asm movq    [edi], mm4  /* here mm1=src1*/  
  266. __asm psrlq mm3, 8 /* mm3 = src3 + '1' ... with [x00000000] at the end*/
  267. }
  270. #define _ABS_DIFF_TRI_prefetch(Z, X) __asm 
  271. { 
  272. __asm movq mm4,mm1 /* mm4=mm1 */ 
  273. __asm add ebx, width  
  274. __asm add edi, imageSize 
  275. __asm por mm3,mm7 /* here mm2=new src2 mm3=new src3 */ 
  277. __asm movq mm7, mm0 
  278. __asm psubusb mm4,mm2 /* mm4 = src1 - src2 */ 
  280. __asm psubusb mm2,mm1 /* mm2 = src2 - src1 */ 
  281. __asm   prefetcht0 [ebx + X] 
  282. __asm psllq mm7,Z
  284. __asm movq mm5,mm1 /* mm5=src1 */ 
  285. __asm por mm4,mm2 /* mm2=|src1-src2| */ 
  288. __asm movq mm2,[ebx] /* mm2= src2 + 'width' = new src2*/ 
  289. __asm psubusb mm5,mm3 /* mm5=src1-src3*/ 
  291. __asm movq mm6,mm3 /* mm6=src3*/ 
  292.     __asm   psubusb mm6,mm1   /* mm3=src3-src1*/ 
  294. __asm por mm6,mm5 /* mm6=|src1-src3|*/ 
  295. __asm paddusb mm4,mm6 /* mm4 = |src1-src2|+|src1-src3|*/ 
  297. __asm movq    [edi], mm4  /* here mm1=src1*/  
  298. __asm psrlq mm3, 8 /* mm3 = src3 + '1' ... with [x00000000] at the end*/
  299. }
  301. //  ImgSubandAdd2: D = saturation0(|S1 - S2| + |S1 - S3|)
  302. // process 8 disparities at a time
  303. //
  304. // Src1: right
  305. // Src2: top
  306. // Src3: left
  307. //
  308. // TODO? divide the result by 2 (shift)
  309. inline int ImgSubandAdd2(const unsigned char *Src1, const unsigned char *Src2, 
  310.  const unsigned char *Src3, 
  311.  unsigned char* Dest1, int l, int imageSize, int width)
  312. {
  313. if (l < 8) return 0;              // image size must be at least 8 bytes 
  314. const int back_step1 = 7*width;
  315. const int back_step2 = 7*imageSize;
  316.   __asm 
  317.   {
  318.         mov eax, Src1     
  319.         mov ebx, Src2
  320. mov edx, Src3
  321.         mov edi, Dest1   
  323.         mov ecx, l   
  324.         shr ecx, 3
  326. movq mm0,[edx] // mm0=src3
  327. movq mm0,[edx] // mm0=src3
  328. align 16
  329. inner_loop:
  330. movq mm1,[eax] // mm1=src1
  331. movq mm3,mm0 // mm3=src3
  333. movq mm2,[ebx] // mm2=src2
  334.         add eax,8         
  335.  
  336. // -- 1 --------- in : mm1,mm2,mm3     out: mm4=SAD  mm2=new mm2 --
  337. movq mm4,mm1 // mm4=mm1
  339. add ebx,width
  341. psubusb mm4,mm2 // mm4 = src1 - src2
  342. //prefetcht0 [ebx + 32 + 2*320]
  344. movq mm0,[edx+8]
  345. psubusb mm2,mm1 // mm2 = src2 - src1
  346.         
  347. movq mm5,mm1 // mm5=src1
  348. por mm4,mm2 // mm2=|src1-src2|
  350. movq mm2,[ebx] // mm2= src2 + 'width' = new src2
  351. psubusb mm5,mm3 // mm5=src1-src3
  353. movq mm6,mm3 // mm6=src3
  354.         psubusb mm6,mm1   // mm3=src3-src1
  356. movq mm7, mm0
  357. psrlq mm3, 8 // mm3 = src3 + '1' ... with [x00000000] at the end
  359. por mm6,mm5 // mm6=|src1-src3|
  360. paddusb mm4,mm6 // mm4 = |src1-src2|+|src1-src3|
  362.         movq    [edi], mm4  
  363. psllq mm7, 56 // here mm1=src1 mm2=NEW src2 mm3=begin of NEWsrc3    mm7=end of NEWsrc3
  364. // -------------------------------------------------------------
  367. // - 2 ----------------
  368. _ABS_DIFF_TRI(48)
  370.   // - 3 ----------------
  371. _ABS_DIFF_TRI(40)
  373.   // - 4 ----------------
  374. _ABS_DIFF_TRI(32)
  375. // _ABS_DIFF_TRI_prefetch(32,24 + 3*320)
  377. // - 5 ----------------
  378. _ABS_DIFF_TRI(24)
  380. // - 6 ----------------
  381. _ABS_DIFF_TRI(16)
  383. // - 7 ----------------
  384. _ABS_DIFF_TRI(8)
  387. // - 8 ----------------
  388. movq mm4,mm1 // mm4=mm1
  389. por mm3,mm7 // here mm2=new src2 mm3=new src3
  391. psubusb mm4,mm2 // mm4 = src1 - src2
  392. psubusb mm2,mm1 // mm2 = src2 - src1
  393.         
  394. movq mm5,mm1 // mm5=src1
  395. por mm4,mm2 // mm2=|src1-src2|
  397. psubusb mm5,mm3 // mm5=src1-src3
  398.         psubusb mm3,mm1   // mm3=src3-src1
  400. por mm3,mm5 // mm6=|src1-src3|
  401. paddusb mm4,mm3 // mm4 = |src1-src2|+|src1-src3|
  403. add edi, imageSize
  405.         movq    [edi], mm4  // here mm1=src1
  406. // -------------------------------------------------------------
  407.   // 
  408. sub ebx, back_step1
  409.         add ebx,8
  410.         add edx,8     
  411. sub edi, back_step2
  412.         add edi,8
  413.         dec ecx      
  414.         jnz inner_loop    
  415.         emms   
  416.   }
  418.   return 1;
  419. }
  422. // macro: in: mm1,mm2
  423. #define _ABS_DIFF_ __asm 
  424. { 
  425. __asm  movq mm4,mm1  /* mm4=mm1 */ 
  426. __asm  psubusb mm4,mm2  /* mm4 = src1 - src2 */ 
  427. __asm  psubusb mm2,mm1  /* mm2 = src2 - src1 */ 
  428. __asm  por  mm4,mm2  /* mm2=|src1-src2| */ 
  429. __asm  add ebx, width 
  430. __asm  add edi, imageSize 
  431.     __asm  movq mm2,[ebx] 
  432. __asm  movq [edi], mm4   /* here mm1=src1  */ 
  433. }
  435. //  ImgSubandAdd2: D = saturation0(|S1 - S2| + |S1 - S3|)
  436. // process 8 disparities at a time
  437. // Src1: right
  438. // Src2: top
  439. // TODO? divide the result by 2 (shift)
  440. inline int ImgSubandAdd2_Vert(const unsigned char *Src1, const unsigned char *Src2, 
  441. unsigned char* Dest1, int l, int imageSize, int width)
  442. {
  444. if (l < 8) return 0;              // image size must be at least 8 bytes 
  445. const int back_step1 = 7*width;
  446. const int back_step2 = 7*imageSize;
  447.   __asm 
  448.   {
  449.         mov eax, Src1     
  450.         mov ebx, Src2
  451.         mov edi, Dest1   
  453.         mov ecx, l   
  454.         shr ecx, 3
  456. align 16
  457. inner_loop:
  459. movq mm1,[eax] // mm1=src1
  460. movq mm2,[ebx] // mm2=src2
  461.         add eax,8         
  462.  
  463. // -- 1 --------- in : mm1,mm2,mm3     out: mm4=SAD  mm2=new mm2 --
  464. _ABS_DIFF_
  465. _ABS_DIFF_
  466. _ABS_DIFF_
  467. _ABS_DIFF_
  468. _ABS_DIFF_
  469. _ABS_DIFF_
  470. _ABS_DIFF_
  472. // - 8 ----------------
  473. movq mm4,mm1 // mm4=mm1
  475. psubusb mm4,mm2 // mm4 = src1 - src2
  476. psubusb mm2,mm1 // mm2 = src2 - src1
  477.         
  478. por mm4,mm2 // mm2=|src1-src2|
  479. add edi, imageSize
  481.         movq    [edi], mm4  // here mm1=src1
  482. // -------------------------------------------------------------
  483.   // 
  484. sub ebx, back_step1
  485.         add ebx,8
  486.   sub edi, back_step2
  487.         add edi,8
  488.         dec ecx      
  489.         jnz inner_loop    
  490.         emms   
  491.   }
  493.   return 1;
  494. }
  496. // macro: in: mm1,mm2
  497. #define _ABS_DIFF_HORIZ(Z) __asm 
  498. { 
  499. __asm  movq mm7, mm0 
  500. __asm  add edi, imageSize 
  501. __asm  movq mm5,mm1 /* mm5=src1 */ 
  502. __asm  psllq mm7, Z 
  503. __asm  psubusb mm5,mm3 /* mm5=src1-src3 */ 
  504. __asm  movq mm6,mm3 /* mm6=src3 */ 
  505.     __asm  psubusb mm6,mm1   /* mm3=src3-src1 */ 
  506. __asm  por mm6,mm5 /* mm6=|src1-src3| */ 
  507.     __asm  movq    [edi], mm6  /* here mm1=src1 */ 
  508. __asm  psrlq mm3, 8 /* mm3 = src3 + '1' ... with [x00000000] at the end */ 
  509. __asm  por mm3,mm7 /* here mm3=new src3 */ 
  510. }
  512. //  ImgSubandAdd2: D = saturation0(|S1 - S2| + |S1 - S3|)
  513. // process 8 disparities at a time
  514. //
  515. // Src1: right
  516. // Src2: top
  517. // Src3: left
  518. //
  519. // TODO? divide the result by 2 (shift)
  520. inline int ImgSubandAdd_Horiz(const unsigned char *rightIm, const unsigned char *leftIm, 
  521.  unsigned char* Dest, int l, int imageSize, int width)
  522. {
  524. if (l < 8) return 0;              // image size must be at least 8 bytes 
  525. const int back_step2 = 7*imageSize;
  526.   __asm 
  527.   {
  528.         mov eax, rightIm     
  529.   mov edx, leftIm
  530.         mov edi, Dest 
  532.         mov ecx, l   
  533.         shr ecx, 3
  535. movq mm0,[edx] // mm0=src3
  536. movq mm0,[edx] // mm0=src3
  537. align 16
  538. inner_loop:
  540. movq mm1,[eax] // mm1=src1
  541. movq mm3,mm0 // mm3=src3
  543. // -- 1 --------- in : mm1,mm2,mm3     out: mm4=SAD  mm2=new mm2 --
  544. movq mm0,[edx+8]
  545.         add eax,8         
  546.    
  547. movq mm5,mm1 // mm5=src1
  548. psubusb mm5,mm3 // mm5=src1-src3
  550. movq mm6,mm3 // mm6=src3
  551.         psubusb mm6,mm1   // mm3=src3-src1
  553. movq mm7, mm0
  554. psrlq mm3, 8 // mm3 = src3 + '1' ... with [x00000000] at the end
  556. por mm6,mm5 // mm6=|src1-src3|
  558.         movq    [edi], mm6  
  559. psllq mm7, 56 // here mm1=src1 mm3=begin of NEWsrc3    mm7=end of NEWsrc3
  560. por mm3,mm7 // here mm3=new src3
  562. // - 2 ----------------
  563. _ABS_DIFF_HORIZ(48)
  564. _ABS_DIFF_HORIZ(40)
  565. _ABS_DIFF_HORIZ(32)
  566. _ABS_DIFF_HORIZ(24)
  567. _ABS_DIFF_HORIZ(16)
  568. _ABS_DIFF_HORIZ(8)
  570. // - 8 ----------------
  571. movq mm5,mm1 // mm5=src1
  572. add edi, imageSize
  574. psubusb mm5,mm3 // mm5=src1-src3
  575.         psubusb mm3,mm1   // mm3=src3-src1
  577. por mm3,mm5 // mm6=|src1-src3|
  578.         movq    [edi], mm3  
  579. // -------------------------------------------------------------
  580.   // 
  581.         add edx,8     
  582. sub edi, back_step2
  583.         add edi,8
  584.         dec ecx      
  585.         jnz inner_loop    
  586.         emms   
  587.   }
  589.   return 1;
  590. }
  593. // ----------------------
  594. // FULL IMAGE, BEST ONLY : Keith's code
  595. inline int findMinimumCorrelation_mmx(
  596. const unsigned char *CurrentCorrelation,
  597. unsigned char CurrentDisparity,
  598. unsigned char *Disparity,
  599. unsigned char *BestCorrelation, int bytecount) 
  600. {
  601. if ((bytecount < 8) || ((bytecount % 8) != 0)) {
  602. return 0;
  603. }
  605. __asm {
  606. // load ecx with the pixelblock count = bytecount / 8
  607. mov ecx, bytecount
  608. shr ecx, 3
  610. // setup mm0 with 8 copies of the disparity constant
  611. mov al,  CurrentDisparity
  612. mov ah,  al
  613. mov bx,  ax
  614. shl eax,  16
  615. mov ax,  bx
  616. movd mm0,  eax
  617. movd mm1,  eax
  618. punpckldq mm0,  mm1
  620. // setup mm1 with 8 copies of the xor constant for unsigned => signed conversion
  621. mov eax, 0x80808080
  622. movd mm1, eax
  623. movd mm2, eax
  624. punpckldq mm1, mm2
  627. // setup the image pointers
  628. mov eax,  BestCorrelation
  629. mov esi,  CurrentCorrelation
  630. mov edi,  Disparity
  632. pixel_loop:
  633. movq mm2, [esi] // current correlation
  634. movq mm3, [eax] // best correlation
  636. // check for updates
  637. movq mm5, mm2 // copy the current correlation
  638. pxor mm5, mm1 // convert from unsigned range to signed range
  640. movq mm6, mm3 // copy the best correlation
  641. pxor mm6, mm1 // convert from unsigned range to signed range
  643. pcmpgtb mm5, mm6 // mm5 := (current signed> best) mask
  644. // 1 indicates current >  best, so keep best
  645. // 0 indicates current <= best, so use new value
  647. // BYPASS
  648. // this phase adds 8 additional instructions, but could skip 2 writes and 1 read
  649. // abort remainder if not updating best correlation
  650. pcmpeqb mm6, mm6 // mm6 = 0xFFFFFFFF
  651. pxor mm6,  mm5 // mm6 = mm5 xor 0xFFFFFFFF = not mm5
  652. // 0 indicates current >  best, so keep best
  653. // 1 indicates current <= best, so use new value
  655. packsswb mm6, mm6 // pack it into the lower dword of mm6 (unsigned saturation)
  656. //  11111111 11111111 => 11111111 some replaced
  657. // 11111111 00000000 => 11111111 some replaced
  658. // 00000000 11111111 => 11111111 some replaced
  659. // 00000000 00000000 => 00000000 no replacements
  661. // we don't need to backup ebx because its not used in this routine
  662. // movd mm7, ebx // make a backup of eax
  663. movd ebx, mm6 // get the saturated mask
  664. test ebx, ebx // test ebx => yields 0 iff no substitutions will occur
  665. // movd ebx, mm7 // restore ebx
  666. jz bypass // store mm4 (second correlation) to [ebx]
  669. // Update best Correlation
  670. movq mm6, mm5 // mm6 := mask
  671. movq mm7, mm5 // mm7 := mask
  673. pand mm6, mm3 // best correlation values to keep
  674. pandn mm7, mm2 // current correlation value to move to best correlation
  676. por mm6, mm7 // merge values
  677. movq [eax], mm6 // store values
  679. // update disparity
  680. movq mm2, [edi] // get disparity map
  681. movq mm6, mm5 // mm6 := mask
  683. pand mm5, mm2 // select disparity map values to keep
  684. pandn mm6, mm0 // select current disparity values to move to disparity map
  686. por mm5, mm6 // merge values
  687. movq [edi], mm5 // store values
  689. bypass:
  690. add eax, 8
  691. add esi, 8
  692. add edi, 8
  694. dec ecx
  695. jnz pixel_loop
  698. emms;
  699. }
  701. return 1;
  702. }
  704. /*int initMinimumCorrelation(
  705. const unsigned char *CurrentCorrelation,
  706. unsigned char disparityInit,
  707. unsigned char *Disparity,
  708. unsigned char *BestCorrelation,
  709. unsigned char *SecondCorrelation,
  710. int bytecount) 
  711. {
  712. for (int i=0; i<bytecount; ++i)
  713. {
  714. BestCorrelation[i]=255;
  715. SecondCorrelation[i]=255;
  716. Disparity[i]=0;
  717. }
  718. return 0;
  719. }*/
  721. inline int initMinimumCorrelation(
  722. const unsigned char *CurrentCorrelation,
  723. unsigned char disparityInit,
  724. unsigned char *Disparity,
  725. unsigned char *BestCorrelation,
  726. unsigned char *SecondCorrelation,
  727. int bytecount) 
  728. {
  729. if ((bytecount < 8) || ((bytecount % 8) != 0)) {
  730. return 0;
  731. }
  733. __asm {
  735. // setup mm0 with 8 copies of the disparity constant
  736. mov al,  disparityInit
  737. mov ah,  al
  738. mov bx,  ax
  739. shl eax,  16
  740. mov ax,  bx
  741. movd mm0,  eax
  742. movd mm1,  eax
  743. punpckldq mm0,  mm1
  745. // load ecx with the pixelblock count = bytecount / 8
  746. mov ecx, bytecount
  747. shr ecx, 3
  749. mov eax,  BestCorrelation
  750. mov ebx,  SecondCorrelation
  751. mov esi,  CurrentCorrelation
  752. mov edx, Disparity
  754. pixel_loop:
  755. movq mm1, [esi]
  756. movq [eax], mm1 // Best = Current
  757. movq [ebx], mm1 // Second = Current
  758. movq [edx], mm0 // Disparity = disparityInit
  760. add eax, 8
  761. add ebx, 8
  762. add edx, 8
  763. add esi, 8
  764. dec ecx
  765. jnz pixel_loop
  767. jmp done
  770. done:
  771. emms;
  772. }
  773. }
  775. inline int findMinimumCorrelation(
  776. const unsigned char *CurrentCorrelation,
  777. unsigned char CurrentDisparity,
  778. unsigned char *Disparity,
  779. unsigned char *BestCorrelation,
  780. unsigned char *SecondCorrelation,
  781. int bytecount) 
  782. {
  783. for (int i=0; i<bytecount; ++i,++CurrentCorrelation,++Disparity,++BestCorrelation, ++SecondCorrelation) 
  784. {
  785. if (*CurrentCorrelation<*BestCorrelation) {
  786. *Disparity = CurrentDisparity;
  787. *SecondCorrelation = *BestCorrelation;
  788. *BestCorrelation = *CurrentCorrelation;
  789. }
  790. }
  791. return 1;
  792. }
  794. // ----------------------
  795. // FULL IMAGE, BEST+SECOND .. Keith's code
  796. inline int findMinimumCorrelation_mmx(
  797. const unsigned char *CurrentCorrelation,
  798. unsigned char CurrentDisparity,
  799. unsigned char *Disparity,
  800. unsigned char *BestCorrelation,
  801. unsigned char *SecondCorrelation,
  802. int bytecount) 
  803. {
  804. if ((bytecount < 8) || ((bytecount % 8) != 0)) {
  805. return 0;
  806. }
  808. __asm {
  809. // load ecx with the pixelblock count = bytecount / 8
  810. mov ecx, bytecount
  811. shr ecx, 3
  813. // setup mm0 with 8 copies of the disparity constant
  814. mov al,  CurrentDisparity
  815. mov ah,  al
  816. mov bx,  ax
  817. shl eax,  16
  818. mov ax,  bx
  819. movd mm0,  eax
  820. movd mm1,  eax
  821. punpckldq mm0,  mm1
  823. // setup mm1 with 8 copies of the xor constant for unsigned => signed conversion
  824. mov eax, 0x80808080
  825. movd mm1, eax
  826. movd mm2, eax
  827. punpckldq mm1, mm2
  830. // setup the image pointers
  831. mov eax,  BestCorrelation
  832. mov ebx,  SecondCorrelation
  833. mov esi,  CurrentCorrelation
  834. mov edi,  Disparity
  836. pixel_loop:
  837. movq mm2, [esi] // current correlation
  838. movq mm4, [ebx] // second correlation
  840. // convert the current correlation from unsigned range to signed range
  841. movq mm5, mm2 // copy the current correlation
  842. pxor mm5, mm1 // convert from unsigned range to signed range
  843. movq mm7, mm5 // copy converted to mm7
  846. // check for second correlation updates
  847. movq mm6, mm4 // copy second best correlation
  848. pxor mm6, mm1 // convert from unsigned range to signed range
  850. pcmpgtb mm7, mm6 // mm7 := (current signed> second best) mask
  852. // BYPASS 1
  853. // skip remainder if second correlation is not to be updated
  854. // this phase adds an addition 8 instructions, but it could save as 1 memory read and 3 writes
  855. pcmpeqb mm6, mm6 // mm6 = 0xFFFFFFFF
  856. pxor mm6,  mm7 // mm6 = mm7 xor 0xFFFFFFFF = not mm7
  857. // 0 indicates current >  second, so keep old value
  858. // 1 indicates current <= second, so use new value
  861. packsswb mm6, mm6 // pack it into the lower dword of mm6 (unsigned saturation)
  862. //  11111111 11111111 => 11111111 some replaced
  863. // 11111111 00000000 => 11111111 some replaced
  864. // 00000000 11111111 => 11111111 some replaced
  865. // 00000000 00000000 => 00000000 no replacements
  867. // don't need to backup edx because its not used in this routine
  868. // movd mm3, edx // make a backup of edx
  869. movd edx, mm6 // get the saturated mask
  870. test edx, edx // test edx => yields 0 iff no replacements will occur
  871. // movd edx, mm3 // restore edx
  872. jz bypass1
  875. // direct update second correlation (get values from current)
  876. // mm7 already has mask
  877. // movq mm6, mm7 // mm6 := mask
  878. // pand mm6, mm4 // second correlation values to keep
  879. // pandn mm7, mm2 // current correlation values to move to second correlation
  880. // por mm6, mm7 // merge value => direct updated second correlation
  881. // movq mm4, mm6 // store values (*** this instruction could be eliminated!) 
  883. pand mm4, mm7 // second correlation values to keep
  884. pandn mm7, mm2 // current correlation values to move to second correlation
  885. por mm4, mm7 // merge value => direct updated second correlation
  888. // check for best correlation updates
  889. movq mm3, [eax] // best correlation
  890. // mm5 has converted current correlation
  891. movq mm6, mm3 // copy the best correlation
  892. pxor mm6, mm1 // convert from unsigned range to signed range
  894. pcmpgtb mm5, mm6 // mm5 := (current signed> best) mask
  895. // 1 indicates current >  best, so keep best
  896. // 0 indicates current <= best, so use new value
  897. // BYPASS 2
  898. // this phase adds 8 additional instructions, but could skip 2 writes and 1 read
  899. // abort remainder if not updating best correlation
  900. pcmpeqb mm6, mm6 // mm6 = 0xFFFFFFFF
  901. pxor mm6,  mm5 // mm6 = mm5 xor 0xFFFFFFFF = not mm5
  902. // 0 indicates current >  best, so keep best
  903. // 1 indicates current <= best, so use new value
  905. packsswb mm6, mm6 // pack it into the lower dword of mm6 (unsigned saturation)
  906. //  11111111 11111111 => 11111111 some replaced
  907. // 11111111 00000000 => 11111111 some replaced
  908. // 00000000 11111111 => 11111111 some replaced
  909. // 00000000 00000000 => 00000000 no replacements
  911. // don't need to backup edx because its not used in this routine
  912. // movd mm7, edx // make a backup of edx
  913. movd edx, mm6 // get the saturated mask
  914. test edx, edx // test edx => yields 0 iff no substitutions will occur
  915. // movd edx, mm7 // restore edx
  916. jz bypass2 // store mm4 (second correlation) to [ebx]
  919. // indirect update second correlation (pushed down from best)
  920. movq mm6, mm5 // mm6 := mask
  921. movq mm7, mm5 // mm7 := mask
  923. pand mm6, mm4 // second correlation values to keep
  924. pandn mm7, mm3 // best correlations to move to second correlation
  926. por mm6, mm7 // merge values
  927. movq [ebx], mm6 // store values
  929. // direct Update best Correlation
  930. movq mm6, mm5 // mm6 := mask
  931. movq mm7, mm5 // mm7 := mask
  933. pand mm6, mm3 // best correlation values to keep
  934. pandn mm7, mm2 // current correlation value to move to best correlation
  936. por mm6, mm7 // merge values
  937. movq [eax], mm6 // store values
  939. // update disparity
  940. movq mm2, [edi] // get disparity map
  941. movq mm6, mm5 // mm6 := mask
  943. pand mm5, mm2 // select disparity map values to keep
  944. pandn mm6, mm0 // select current disparity values to move to disparity map
  946. por mm5, mm6 // merge values
  947. movq [edi], mm5 // store values
  950. bypass1:
  951. next_pixel:
  952. add eax, 8
  953. add ebx, 8
  954. add esi, 8
  955. add edi, 8
  957. dec ecx
  958. jnz pixel_loop
  960. jmp done
  962. bypass2:
  963. movq [ebx], mm4;
  964. jmp next_pixel
  966. done:
  967. emms;
  968. }
  970. return 1;
  971. }
  975. inline void sum_Row(uchar* im, unsigned short* im_out, int rowSize, int maskSize) 
  976. {
  977. im += maskSize/2;
  978. im_out += maskSize/2;
  979. for (int i=0; i<rowSize; ++i) {
  980. int s=0;
  981. for (int j=-maskSize/2; j<=maskSize/2; ++j) {
  982. s+=*(im+j);
  983. }
  984. *im_out=s/maskSize;
  985. ++im;++im_out;
  986. }
  987. }
  989. inline void sum_Row_mmx(uchar* im, unsigned short* im_out, int rowSize, int maskSize) 
  990. {
  991. sum_Row_5_mmx(im, im_out, rowSize);
  992. for (int i=0; i<(maskSize-5)/2; ++i)
  993. sum_Row_5_mmx(im_out, im_out, rowSize);
  994. }
  996. inline  void sum_Row_mmx(unsigned short* im, unsigned short* im_out, int rowSize, int maskSize) 
  997. {
  998. sum_Row_5_mmx(im, im_out, rowSize);
  999. for (int i=0; i<(maskSize-5)/2; ++i)
  1000. sum_Row_5_mmx(im_out, im_out, rowSize);
  1001. }
  1003. #define aim_Sum_Words_In_MM1 __asm 
  1004. {
  1005. __asm movq mm4, mm1 
  1006. __asm movq mm2, mm1 
  1008. __asm movq mm3, mm1 
  1009. __asm psllq mm1, 16 
  1011. __asm psrlq mm2, 16 
  1012. __asm paddw mm4, mm2 
  1014. __asm paddw mm3, mm1 
  1015. __asm psrlq mm2, 16 
  1017. __asm psllq mm1, 16 
  1018. __asm paddw mm4, mm2 
  1020. __asm psrlq mm2, 16 
  1021. __asm paddw mm3, mm1 
  1023. __asm psllq mm1, 16 
  1024. __asm paddw mm4, mm2 
  1026. __asm paddw mm3, mm1 
  1027. }
  1033. // apply the mask [1 1 1 1 1] to the 1-D array im (bytes)
  1034. // output : im_out (words)
  1035. inline void sum_Row_5_mmx(uchar* im, unsigned short* im_out, int rowSize) 
  1036. {
  1037. // temp: for debugging
  1038. //return sum_Row_5(im,im_out,rowSize);
  1039. __asm {
  1041. mov eax, rowSize
  1042. mov ebx, im
  1043. mov ecx, im_out
  1045. pxor mm6, mm6 // mm6 = x00000000
  1047. //Process the first quad word, but save only the second result" 
  1048. test eax, eax // Is there anything to do?" 
  1049. jz end_sum_loop // Jump out if necessary 
  1052. //Process low word 
  1053. movq mm1, [ebx] // Copy... 
  1054. punpcklbw mm1, mm6 // Expand low word bytes into words // mm1 =[D C B A]
  1056. aim_Sum_Words_In_MM1
  1058. //Store the result Only in the accumulator 
  1059. movq mm7, mm4 // Update accumulator    mm4=[D  C+D   B+C+D   A+B+C+D]
  1061. //Process high word 
  1062. movq mm1, [ebx] // Copy... 
  1063. punpckhbw mm1, mm6 // Expand high word bytes into words   // mm1 =[H G F E]
  1064. add ebx, 8 // Update input pointer 
  1066.     aim_Sum_Words_In_MM1
  1068. //Add to the previous data ... 
  1069. // mm3=[E+F+G+H  E+F+G  E+F   E]
  1070. // mm4=[H G+H    F+G+H   E+F+G+H]
  1071. paddw mm7, mm3 // The current word of the accum   // mm7=[D+E+F+G+H  C+D+E+F+G  B+C+D+E+F  A+B+C+D+E]
  1073. // translate everything to 2 words on the left
  1074. movq mm1, mm7 // mm1 = [D+E+F+G+H  C+D+E+F+G  B+C+D+E+F  A+B+C+D+E]
  1075. psrlq   mm1, 32 // mm1 = [0  0  D+E+F+G+H  C+D+E+F+G]
  1077. movq mm0, mm1 // mm0 = [D+E+F+G+H  C+D+E+F+G]
  1079. psllq mm7, 32 // mm7 = [B+C+D+E+F  A+B+C+D+E  0   0]
  1081. movq [ecx], mm7 // Store the final result 
  1082. add ecx, 8 // Update output pointer 
  1084. movq mm7, mm4 // Update accumulator    mm4=[H G+H    F+G+H   E+F+G+H]
  1085. sub eax, 8 // Update the number of points left 
  1087. // Start the loop 
  1088. row_sum_loop:
  1089. test eax, eax // Is there anything to do? 
  1090. jz end_sum_loop // Jump out if necessary 
  1092. movq mm1, [ebx] // Load data 
  1094. //Process low word 
  1095. punpcklbw mm1, mm6 // Expand low word bytes into words 
  1097. aim_Sum_Words_In_MM1
  1099. //Add to the previous data 
  1100. //prefetcht1 [ecx+16]
  1101. paddw mm7, mm3 // The current word of the accum 
  1103. // translate everything to 2 words on the left
  1104. // mm0 = [0 0 D C]  mm7 = [H G F E] ----> mm7=[0 0 H G]  [ecx]=[F E D C]
  1105. punpckldq mm0, mm7 // mm0 = [F E D C]
  1107. movq [ecx], mm0
  1108. sub eax, 8 // Update the number of points left 
  1110. movq mm0, mm4 // Update accumulator 
  1111. psrlq mm7, 32 // mm7 = [0 0 H G]
  1113. //Process high word 
  1114. movq mm1, [ebx] // Copy... 
  1115. punpckhbw mm1, mm6 // Expand high word bytes into words 
  1117. aim_Sum_Words_In_MM1
  1119. //Add to the previous data 
  1120. paddw mm0, mm3 // The current word of the accum 
  1122. // translate everything to 2 words on the left
  1123. // mm7 = [0 0 D C]  mm0 = [H G F E] ----> mm0=[0 0 H G]  [ecx+8]=[F E D C]
  1124. punpckldq mm7, mm0 // mm7 = [F E D C]
  1125. add ebx, 8 // Update input pointer 
  1127. movq [ecx+8], mm7
  1128. psrlq mm0, 32 // mm0 = [0 0 H G] 
  1130. movq mm7, mm4 // Update accumulator 
  1131. add ecx, 16 // Update output pointer 
  1133. jmp row_sum_loop // Loop 
  1135. //Cleanup 
  1136. end_sum_loop:
  1137. emms 
  1138. }
  1139. }
  1143. // apply the mask (1/4)*[1 1 1 1 1] to the 1-D array im (words)
  1144. // output : im_out (words)
  1145. inline void sum_Row_5_mmx(ushort* im, ushort* im_out, int rowSize) 
  1146. {
  1147. // temp: for debugging
  1148. //return sum_Row_5(im,im_out,rowSize);
  1149. __asm {
  1151. mov eax, rowSize
  1152. mov ebx, im
  1153. mov ecx, im_out
  1155. //Process the first quad word, but save only the second result" 
  1156. test eax, eax // Is there anything to do?" 
  1157. jz end_sum_loop // Jump out if necessary 
  1159. movq mm1, [ebx] // Load data (4 words)
  1160. add ebx, 8 // Update input pointer 
  1162. //Process low word 
  1163. aim_Sum_Words_In_MM1
  1165. //Store the result Only in the accumulator 
  1166. movq mm7, mm4 // Update accumulator 
  1168. //Process high word 
  1169. movq mm1, [ebx] // Copy... 
  1171. aim_Sum_Words_In_MM1
  1172. add ebx, 8
  1174. //Add to the previous data 
  1175. paddw mm7, mm3 // The current word of the accum 
  1177. // translate everything to 2 words on the left
  1178. movq mm1, mm7 // mm1 = [D+E+F+G+H  C+D+E+F+G  B+C+D+E+F  A+B+C+D+E]
  1179. psrlq   mm1, 32 // mm1 = [0  0  D+E+F+G+H  C+D+E+F+G]
  1180. movq mm0, mm1 // mm0 = [0  0  D+E+F+G+H  C+D+E+F+G]
  1181. psllq mm7, 32 // mm7 = [B+C+D+E+F  A+B+C+D+E  0   0]
  1183. movq [ecx], mm7 // Store the final result 
  1184. movq mm7, mm4 // Update accumulator 
  1186. add ecx, 8 // Update output pointer 
  1187. sub eax, 8 // Update the number of points left 
  1189. // Start the loop 
  1190. row_sum_loop:
  1191. test eax, eax // Is there anything to do? 
  1192. jz end_sum_loop // Jump out if necessary 
  1194. movq mm1, [ebx] // Load data 
  1196. aim_Sum_Words_In_MM1
  1198. //Add to the previous data 
  1199. //prefetcht0 [ecx + 32]
  1200. //prefetcht0 [ebx + 48]
  1201. paddw mm7, mm3 // The current word of the accum 
  1202. psrlw mm7, 2 // divide result by ...
  1204. // translate everything to 2 words on the left
  1205. // mm0 = [0 0 D C]  mm7 = [H G F E] ----> mm7 =[0 0 H G]  [ecx]=[F E D C]
  1206. punpckldq mm0, mm7  // mm0 = [F E D C]
  1208. movq [ecx], mm0
  1209. sub eax, 8 // Update the number of points left 
  1211. movq mm0, mm4 // Update accumulator 
  1212. psrlq mm7, 32 // mm7 =[0 0 H G]
  1214. //Process high word 
  1215. movq mm1, [ebx+8] // Copy... 
  1217. aim_Sum_Words_In_MM1
  1219. //Add to the previous data 
  1220. paddw mm0, mm3 // The current word of the accum 
  1221. psrlw mm0, 2 // divide result by ...
  1223. // translate everything to 2 words on the left
  1224. // mm7 = [0 0 D C]  mm0 = [H G F E] ----> mm0=[0 0 H G]  [ecx+8]=[F E D C]
  1225. punpckldq mm7, mm0 // mm7 = [F E D C]
  1226. add ebx, 16 // Update input pointer 
  1228. movq [ecx+8], mm7
  1229. psrlq mm0, 32 // mm0 = [0 0 H G]
  1231. movq mm7, mm4 // Update accumulator 
  1232. add ecx, 16 // Update output pointer */
  1234. jmp row_sum_loop // Loop 
  1236. //Cleanup 
  1237. end_sum_loop:
  1238. emms 
  1239. }
  1240. }
  1242. template<class T> void sum_Row_5(T* im, ushort* im_out, int rowSize)
  1243. {
  1244. im += 2;
  1245. im_out +=2;
  1246. int s = 0;
  1247. for (int i=0; i<rowSize-5; ++i, ++im, ++im_out) {
  1248. s = *(im-2);
  1249. s += *(im-1);
  1250. s += *(im);
  1251. s += *(im+1);
  1252. s += *(im+2);
  1253. *im_out = s/5;
  1254. }
  1255. }
  1257. inline void avg_Col(ushort* im, uchar* im_out, int dataSize, int width, int sizeMask)
  1258. {
  1259. int offset = width*(sizeMask/2);
  1260. im += offset;
  1261. im_out += offset;
  1262. for (int i=0; i<dataSize-width*sizeMask; ++i, ++im, ++im_out) {
  1263. int s = 0;
  1264. for (int j=-sizeMask/2; j<=sizeMask/2; ++j) s += *(im+j*width);
  1265. *im_out = s/(sizeMask);
  1266. }
  1267. }
  1269. // apply vertical mask 1/16*[1 1 1 ... 1]^T to 'im'
  1270. // result in 'im_out'
  1271. inline void avg_Col_mmx(ushort* im, uchar* im_out, int dataSize, int width, int sizeMask)
  1272. {
  1273. // temp: for debugging
  1274. //return avg_Col(im,im_out,dataSize,width,sizeMask);
  1276. switch (sizeMask)
  1277. {
  1278. case 5: avg_Col_5(im,im_out,dataSize,width);
  1279. break;
  1280. case 7: avg_Col_7(im,im_out,dataSize,width);
  1281. break;
  1282. case 9: avg_Col_9(im,im_out,dataSize,width);
  1283. break;
  1284. case 11: avg_Col_11(im,im_out,dataSize,width);
  1285. break;
  1286. case 13: avg_Col_13(im,im_out,dataSize,width);
  1287. break;
  1288. case 15: avg_Col_15(im,im_out,dataSize,width);
  1289. break;
  1290. case 17: avg_Col_17(im,im_out,dataSize,width);
  1291. break;
  1293. default: if (sizeMask<5) avg_Col_5(im,im_out,dataSize,width);
  1294.  else if (sizeMask>17) avg_Col_17(im,im_out,dataSize,width);
  1295. break;
  1297. }
  1298. }
  1302. #define macro_add __asm 
  1303. {
  1304. __asm  paddusw mm3, [edx]
  1305. __asm paddusw mm2, [edx+8]  
  1306. __asm add edx, edi
  1307. }
  1310. inline void avg_Col_5(ushort* im, uchar* im_out, int dataSize, int width)
  1311. {
  1312. __asm {
  1314. mov edi, width
  1315. shl edi, 1  // edi = 2*width
  1317. mov eax, dataSize
  1318. mov ecx, im_out
  1320. mov ebx, im
  1321. sub ebx, edi
  1322. sub ebx, edi // ebx = ebx-4*width
  1324. test eax, eax // Is there anything to do?" 
  1325. jz end_sum_loop // Jump out if necessary 
  1327. row_sum_loop:
  1329. test eax, eax // Is there anything to do? 
  1330. jz end_sum_loop // Jump out if necessary 
  1332. mov edx, ebx
  1333. add ebx, 16
  1335. // 1
  1336. movq mm3, [edx] // mm3 = 4 words of im
  1337. movq mm2, [edx+8] // mm2 = next 4 words of im
  1338. add edx, edi
  1340. macro_add
  1341. macro_add
  1342. macro_add
  1343. macro_add
  1345. // divide results by ...
  1346. psrlw mm3, 3
  1347. psrlw mm2, 3
  1349. // convert [mm2 mm3] as 8 bytes
  1350. packuswb mm3,mm2
  1351. movq [ecx], mm3
  1353. sub eax, 8 // Update the number of points left 
  1354. add ecx, 8 // Update output pointer 
  1356. jmp row_sum_loop // Loop 
  1358. //Cleanup 
  1359. end_sum_loop:
  1360. emms 
  1361. }
  1362. }
  1364. inline void avg_Col_7(ushort* im, uchar* im_out, int dataSize, int width)
  1365. {
  1366. __asm {
  1368. mov edi, width
  1369. shl edi, 1  // edi = 2*width
  1371. mov eax, dataSize
  1372. mov ecx, im_out
  1374. mov ebx, im
  1375. sub ebx, edi
  1376. sub ebx, edi
  1377. sub ebx, edi // ebx = ebx-6*width
  1379. test eax, eax // Is there anything to do?" 
  1380. jz end_sum_loop // Jump out if necessary 
  1382. row_sum_loop:
  1384. test eax, eax // Is there anything to do? 
  1385. jz end_sum_loop // Jump out if necessary 
  1387. mov edx, ebx
  1389. // 1
  1390. movq mm3, [edx] // mm3 = 4 words of im
  1391. add ebx, 16
  1392. movq mm2, [edx+8] // mm2 = next 4 words of im
  1393. add edx, edi
  1395. macro_add
  1396. macro_add
  1397. macro_add
  1398. macro_add
  1399. macro_add
  1400. macro_add
  1402. // divide results by ...
  1403. psrlw mm3, 3
  1404. psrlw mm2, 3
  1406. // convert [mm2 mm3] as 8 bytes
  1407. packuswb mm3,mm2
  1408. movq [ecx], mm3
  1410. sub eax, 8 // Update the number of points left 
  1411. add ecx, 8 // Update output pointer 
  1413. jmp row_sum_loop // Loop 
  1415. //Cleanup 
  1416. end_sum_loop:
  1417. emms 
  1418. }
  1419. }
  1421. inline void avg_Col_9(ushort* im, uchar* im_out, int dataSize, int width)
  1422. {
  1423. __asm {
  1425. mov edi, width
  1426. shl edi, 1  // edi = 2*width
  1428. mov eax, dataSize
  1429. mov ecx, im_out
  1431. mov ebx, im
  1432. sub ebx, edi
  1433. sub ebx, edi
  1434. sub ebx, edi
  1435. sub ebx, edi // ebx = ebx-8*width
  1437. test eax, eax // Is there anything to do?" 
  1438. jz end_sum_loop // Jump out if necessary 
  1440. row_sum_loop:
  1442. test eax, eax // Is there anything to do? 
  1443. jz end_sum_loop // Jump out if necessary 
  1445. mov edx, ebx
  1446. add ebx, 16
  1448. // 1
  1449. movq mm3, [edx] // mm3 = 4 words of im
  1450. movq mm2, [edx+8] // mm2 = next 4 words of im
  1451. add edx, edi
  1453. macro_add
  1454. macro_add
  1455. macro_add
  1456. macro_add
  1457. macro_add
  1458. macro_add
  1459. macro_add
  1460. macro_add
  1462. // divide results by ...
  1463. psrlw mm3, 3
  1464. psrlw mm2, 3
  1466. // convert [mm2 mm3] as 8 bytes
  1467. packuswb mm3,mm2
  1468. movq [ecx], mm3
  1470. sub eax, 8 // Update the number of points left 
  1471. add ecx, 8 // Update output pointer 
  1473. jmp row_sum_loop // Loop 
  1475. //Cleanup 
  1476. end_sum_loop:
  1477. emms 
  1478. }
  1479. }
  1481. inline void avg_Col_11(ushort* im, uchar* im_out, int dataSize, int width)
  1482. {
  1483. __asm {
  1485. mov edi, width
  1486. shl edi, 1  // edi = 2*width
  1488. mov eax, dataSize
  1489. mov ecx, im_out
  1491. mov ebx, im
  1492. sub ebx, edi
  1493. sub ebx, edi
  1494. sub ebx, edi
  1495. sub ebx, edi 
  1496. sub ebx, edi // ebx = ebx-10*width
  1498. test eax, eax // Is there anything to do?" 
  1499. jz end_sum_loop // Jump out if necessary 
  1501. row_sum_loop:
  1503. test eax, eax // Is there anything to do? 
  1504. jz end_sum_loop // Jump out if necessary 
  1506. mov edx, ebx
  1507. add ebx, 16
  1509. // 1
  1510. movq mm3, [edx] // mm3 = 4 words of im
  1511. movq mm2, [edx+8] // mm2 = next 4 words of im
  1512. add edx, edi
  1514. macro_add
  1515. macro_add
  1516. macro_add
  1517. macro_add
  1518. macro_add
  1519. macro_add
  1520. macro_add
  1521. macro_add
  1522. macro_add
  1523. macro_add
  1525. // divide results by ...
  1526. psrlw mm3, 4
  1527. psrlw mm2, 4
  1529. // convert [mm2 mm3] as 8 bytes
  1530. packuswb mm3,mm2
  1531. movq [ecx], mm3
  1533. sub eax, 8 // Update the number of points left 
  1534. add ecx, 8 // Update output pointer 
  1536. jmp row_sum_loop // Loop 
  1538. //Cleanup 
  1539. end_sum_loop:
  1540. emms 
  1541. }
  1542. }
  1544. inline void avg_Col_13(ushort* im, uchar* im_out, int dataSize, int width)
  1545. {
  1546. __asm {
  1548. mov edi, width
  1549. shl edi, 1  // edi = 2*width
  1551. mov eax, dataSize
  1552. mov ecx, im_out
  1554. mov ebx, im
  1555. sub ebx, edi
  1556. sub ebx, edi
  1557. sub ebx, edi
  1558. sub ebx, edi 
  1559. sub ebx, edi
  1560. sub ebx, edi // ebx = ebx-12*width
  1562. test eax, eax // Is there anything to do?" 
  1563. jz end_sum_loop // Jump out if necessary 
  1565. row_sum_loop:
  1567. test eax, eax // Is there anything to do? 
  1568. jz end_sum_loop // Jump out if necessary 
  1570. mov edx, ebx
  1571. add ebx, 16
  1573. // 1
  1574. movq mm3, [edx] // mm3 = 4 words of im
  1575. movq mm2, [edx+8] // mm2 = next 4 words of im
  1576. add edx, edi
  1578. macro_add
  1579. macro_add
  1580. macro_add
  1581. macro_add
  1582. macro_add
  1583. macro_add
  1584. macro_add
  1585. macro_add
  1586. macro_add
  1587. macro_add
  1588. macro_add
  1589. macro_add
  1591. // divide results by ...
  1592. psrlw mm3, 4
  1593. psrlw mm2, 4
  1595. // convert [mm2 mm3] as 8 bytes
  1596. packuswb mm3,mm2
  1597. movq [ecx], mm3
  1599. sub eax, 8 // Update the number of points left 
  1600. add ecx, 8 // Update output pointer 
  1602. jmp row_sum_loop // Loop 
  1604. //Cleanup 
  1605. end_sum_loop:
  1606. emms 
  1607. }
  1608. }
  1610. inline void avg_Col_15(ushort* im, uchar* im_out, int dataSize, int width)
  1611. {
  1612. __asm {
  1614. mov edi, width
  1615. shl edi, 1  // edi = 2*width
  1617. mov eax, dataSize
  1618. mov ecx, im_out
  1620. mov ebx, im
  1621. sub ebx, edi
  1622. sub ebx, edi
  1623. sub ebx, edi
  1624. sub ebx, edi
  1625. sub ebx, edi 
  1626. sub ebx, edi
  1627. sub ebx, edi // ebx = ebx-14*width
  1629. test eax, eax // Is there anything to do?" 
  1630. jz end_sum_loop // Jump out if necessary 
  1632. row_sum_loop:
  1634. test eax, eax // Is there anything to do? 
  1635. jz end_sum_loop // Jump out if necessary 
  1637. mov edx, ebx
  1638. add ebx, 16
  1640. // 1
  1641. movq mm3, [edx] // mm3 = 4 words of im
  1642. movq mm2, [edx+8] // mm2 = next 4 words of im
  1643. add edx, edi
  1645. macro_add
  1646. macro_add
  1647. macro_add
  1648. macro_add
  1649. macro_add
  1650. macro_add
  1651. macro_add
  1652. macro_add
  1653. macro_add
  1654. macro_add
  1655. macro_add
  1656. macro_add
  1657. macro_add
  1658. macro_add
  1660. // divide results by ...
  1661. psrlw mm3, 4
  1662. psrlw mm2, 4
  1664. // convert [mm2 mm3] as 8 bytes
  1665. packuswb mm3,mm2
  1666. movq [ecx], mm3
  1668. sub eax, 8 // Update the number of points left 
  1669. add ecx, 8 // Update output pointer 
  1671. jmp row_sum_loop // Loop 
  1673. //Cleanup 
  1674. end_sum_loop:
  1675. emms 
  1676. }
  1677. }
  1679. inline void avg_Col_17(ushort* im, uchar* im_out, int dataSize, int width)
  1680. {
  1681. __asm {
  1683. mov edi, width
  1684. shl edi, 1  // edi = 2*width
  1686. mov eax, dataSize
  1687. mov ecx, im_out
  1689. mov ebx, im
  1690. sub ebx, edi
  1691. sub ebx, edi
  1692. sub ebx, edi
  1693. sub ebx, edi
  1694. sub ebx, edi
  1695. sub ebx, edi 
  1696. sub ebx, edi
  1697. sub ebx, edi // ebx = ebx-16*width
  1699. test eax, eax // Is there anything to do?" 
  1700. jz end_sum_loop // Jump out if necessary 
  1702. row_sum_loop:
  1704. test eax, eax // Is there anything to do? 
  1705. jz end_sum_loop // Jump out if necessary 
  1707. mov edx, ebx
  1708. add ebx, 16
  1710. // 1
  1711. movq mm3, [edx] // mm3 = 4 words of im
  1712. movq mm2, [edx+8] // mm2 = next 4 words of im
  1713. add edx, edi
  1715. macro_add
  1716. macro_add
  1717. macro_add
  1718. macro_add
  1719. macro_add
  1720. macro_add
  1721. macro_add
  1722. macro_add
  1723. macro_add
  1724. macro_add
  1725. macro_add
  1726. macro_add
  1727. macro_add
  1728. macro_add
  1729. macro_add
  1730. macro_add
  1732. // divide results by ...
  1733. psrlw mm3, 4
  1734. psrlw mm2, 4
  1736. // convert [mm2 mm3] as 8 bytes
  1737. packuswb mm3,mm2
  1738. movq [ecx], mm3
  1740. sub eax, 8 // Update the number of points left 
  1741. add ecx, 8 // Update output pointer 
  1743. jmp row_sum_loop // Loop 
  1745. //Cleanup 
  1746. end_sum_loop:
  1747. emms 
  1748. }
  1749. }
  1752. inline void add_Col_5_wb(ushort* im, uchar* im_out, int dataSize, int width)
  1753. {
  1754. __asm {
  1756. mov edi, width
  1757. shl edi, 1  // edi = 2*width
  1759. mov eax, dataSize
  1760. mov ecx, im_out
  1762. mov ebx, im
  1763. sub ebx, edi
  1764. sub ebx, edi // ebx = ebx-4*width
  1766. test eax, eax // Is there anything to do?" 
  1767. jz end_sum_loop // Jump out if necessary 
  1769. row_sum_loop:
  1771. test eax, eax // Is there anything to do? 
  1772. jz end_sum_loop // Jump out if necessary 
  1774. mov edx, ebx
  1775. add ebx, 16
  1777. // 1
  1778. movq mm3, [edx] // mm3 = 4 words of im
  1779. movq mm2, [edx+8] // mm2 = next 4 words of im
  1780. add edx, edi
  1782. macro_add
  1783. macro_add
  1784. macro_add
  1785. macro_add
  1787. // save [mm2 mm3] as 8 bytes
  1788. packuswb mm3,mm2
  1789. movq [ecx], mm3
  1791. sub eax, 8 // Update the number of points left 
  1792. add ecx, 8 // Update output pointer 
  1794. jmp row_sum_loop // Loop 
  1796. //Cleanup 
  1797. end_sum_loop:
  1798. emms 
  1799. }
  1800. }
  1802. inline void add_Col_5_ww(ushort* im, ushort* im_out, int dataSize, int width)
  1803. {
  1804. __asm {
  1806. mov edi, width
  1807. shl edi, 1  // edi = 2*width
  1809. mov eax, dataSize
  1810. mov ecx, im_out
  1812. mov ebx, im
  1813. sub ebx, edi
  1814. sub ebx, edi // ebx = ebx-4*width
  1816. test eax, eax // Is there anything to do?" 
  1817. jz end_sum_loop // Jump out if necessary 
  1819. row_sum_loop:
  1821. test eax, eax // Is there anything to do? 
  1822. jz end_sum_loop // Jump out if necessary 
  1824. mov edx, ebx
  1825. add ebx, 16
  1827. // 1
  1828. movq mm3, [edx] // mm3 = 4 words of im
  1829. movq mm2, [edx+8] // mm2 = next 4 words of im
  1830. add edx, edi
  1832. macro_add
  1833. macro_add
  1834. macro_add
  1835. macro_add
  1837. // save [mm2 mm3] as words
  1838. movq [ecx], mm3
  1839. movq [ecx+8], mm2
  1841. sub eax, 8 // Update the number of points left 
  1842. add ecx, 16 // Update output pointer 
  1844. jmp row_sum_loop // Loop 
  1846. //Cleanup 
  1847. end_sum_loop:
  1848. emms 
  1849. }
  1850. }
  1852. // compare bestScores and secondScores. if second<best+'thresh' the disp.
  1853. // is set to 'valForReplacement' (usually 0)
  1854. inline void compareBestAndSecond(uchar* bestScores, uchar* secondScores, char thresh, 
  1855.   uchar undefined_val, 
  1856.   uchar* disp, int dataSize)
  1857. {
  1858. __asm {
  1860. // setup mm0 with 8 copies of 'thresh'
  1861. mov al,  thresh
  1862. mov ah,  al
  1863. mov bx,  ax
  1864. shl eax,  16
  1865. mov ax,  bx
  1866. movd mm0,  eax
  1867. movd mm1,  eax
  1868. punpckldq mm0,  mm1
  1870. // setup mm7 with 8 copies of 'valForReplacement'
  1871. mov al,  undefined_val
  1872. mov ah,  al
  1873. mov bx,  ax
  1874. shl eax,  16
  1875. mov ax,  bx
  1876. movd mm7,  eax
  1877. movd mm1,  eax
  1878. punpckldq mm7,  mm1
  1880. mov eax, dataSize
  1881. mov ebx, bestScores
  1882. mov ecx, secondScores
  1883. mov edx, disp
  1885. test eax, eax // Is there anything to do?" 
  1886. jz end_loop // Jump out if necessary 
  1888. comp_loop:
  1890. test eax, eax // Is there anything to do? 
  1891. jz end_loop // Jump out if necessary 
  1893. movq mm2, [ecx]
  1894. psubusb mm2, [ebx] // mm2 = secondScores - bestScores
  1896. movq mm3, [edx] // mm3 = disp
  1897. pcmpgtb mm2, mm0 // mm2 = 1 if mm2>thresh
  1898. //  0 otherwise
  1900. pand mm3, mm2
  1901. pandn mm2, mm7
  1903. por mm3, mm2
  1904. movq [edx], mm3
  1906. sub eax, 8 // Update the number of points left 
  1907. add ebx, 8 // Update output pointer 
  1908. add ecx, 8 
  1909. add edx, 8
  1911. jmp comp_loop // Loop 
  1913. //Cleanup 
  1914. end_loop:
  1915. emms 
  1916. }
  1917. }
  1919. // windowWidth must be multiple of 8
  1920. inline void cropImage(const uchar* imSrc, int width, int height, 
  1921.    uchar* imDest, int x0, int y0, int windowWidth, int windowHeight)
  1922. {
  1923. int w8 = windowWidth/8;
  1925. int step = width-windowWidth;
  1926. const uchar* srcNewOrigin = imSrc+x0+y0*width;
  1928. __asm {
  1930. mov ecx, windowHeight
  1932. mov edx, w8
  1933. mov eax,  srcNewOrigin
  1934. mov ebx,  imDest
  1936. pixel_loop:
  1938. movq mm1, [eax]
  1939. movq [ebx], mm1
  1940. add eax, 8
  1941. add ebx, 8
  1943. dec edx
  1944. jnz pixel_loop
  1946. mov edx, w8
  1947. add eax, step
  1949. dec ecx
  1950. jnz pixel_loop
  1952. jmp done
  1954. done:
  1955. emms;
  1956. }
  1957. }
  1959. // return the average pixel value
  1960. inline float pixelMean(const uchar* im, int imageSize)
  1961. {
  1962. int sum;
  1964. __asm {
  1966. mov ecx, imageSize
  1967. shr ecx, 3
  1969. mov eax, im
  1970. pxor mm7,mm7 // mm7 used as accumulator
  1971. pxor mm0,mm0 // mm0 = 0
  1973. pixel_loop:
  1975. movq mm1, [eax]
  1976. movq mm2,mm1
  1978. punpcklbw mm2, mm0
  1979. punpckhbw mm1, mm0
  1981. paddw mm2,mm1
  1983. movq mm1,mm2
  1984. punpcklwd mm2, mm0
  1985. punpckhwd mm1, mm0
  1987. paddd mm2,mm1
  1988. paddd mm7,mm2
  1990. add eax, 8
  1991. dec ecx
  1992. jnz pixel_loop
  1994. jmp done
  1996. done:
  1997. movd ebx, mm7
  1998. psrlq mm7, 32
  1999. movd edx, mm7
  2000. add ebx, edx
  2001. mov sum, ebx
  2003. emms
  2004. }
  2006. return sum / (float)imageSize;
  2007. }
  2012. // -------------------------------------------------------------
  2013. // apply mask:
  2014. // if mask[]=undefined_val im[]->im[]
  2015. // otherwise, im[]->mask[]
  2016. // ....... this one may not be exact :-(
  2017. inline void overrideImageMMX(uchar* im, const uchar* mask, uchar undefined_val, int imageSize)
  2018. {
  2019. __asm {
  2020. // setup mm0 with 8 copies of 'undefined_val'
  2021. mov al,  undefined_val
  2022. mov ah,  al
  2023. mov bx,  ax
  2024. shl eax,  16
  2025. mov ax,  bx
  2026. movd mm0,  eax
  2027. movd mm1,  eax
  2028. punpckldq mm0,  mm1
  2030. mov ecx, imageSize
  2031. shr ecx, 3
  2033. mov eax, im
  2034. mov ebx, mask
  2036. pixel_loop:
  2037. movq mm1, [eax]
  2038. movq mm2, [ebx]
  2040. movq mm3, mm2
  2041. pcmpeqb mm3, mm0 // mm3[] -> xFF if mm2[]==undefined_val
  2042. // -> x00 otherwise
  2043. pand mm3, mm1 // mm3[] = mm1[] if mm2[]==undefined_val
  2044. //  = x00   otherwise
  2045. por mm3, mm2
  2046. movq [eax], mm3
  2048. add eax, 8
  2049. add ebx, 8
  2050. dec ecx
  2051. jnz pixel_loop
  2053. jmp done
  2055. done:
  2056. emms
  2057. }
  2058. }
  2060. inline void overrideImage(uchar* im, const uchar* mask, uchar undefined_val, int imageSize)
  2061. {
  2062. for (int i=0; i<imageSize; ++i, ++im,++mask)
  2063. {
  2064. if (*mask != undefined_val) *im=*mask;
  2065. }
  2067. }
  2070. inline void divide( ushort* im,  uchar* div, uchar* result, int imageSize)
  2071. {
  2072. for (int i=0; i<imageSize; ++i,++im,++div,++result)
  2073. {
  2074. *result = (*div)?(uchar)(*im / *div):0;
  2075. }
  2076. }
  2078. // 5x5 sum filters
  2079. inline void sum_5x5_mmx( uchar* im, ushort* im_out, int dataSize, int width, ushort* buff)
  2080. {
  2081. sum_Row_5_mmx(im, buff, dataSize);
  2082. add_Col_5_ww(buff+2*width, im_out+2*width, dataSize-4*width , width);
  2083. }
  2085. inline void sum_5x5_mmx( uchar* im, uchar* im_out, int dataSize, int width, ushort* buff)
  2086. {
  2087. sum_Row_5_mmx(im, buff, dataSize);
  2088. add_Col_5_wb(buff+2*width, im_out+2*width, dataSize-4*width , width);
  2089. }
  2092. inline void binarize(uchar* im, uchar* im_out, uchar undefined_val, int dataSize)
  2093. {
  2094. for (int i=0; i<dataSize; ++i,++im,++im_out)
  2095. {
  2096. *im_out = (*im != undefined_val);
  2097. }
  2098. }
  2100. inline void set_undefined_to_zero(uchar* im, uchar* im_out, uchar undefined_val, int dataSize)
  2101. {
  2102. for (int i=0; i<dataSize; ++i,++im,++im_out)
  2103. {
  2104. if (*im == undefined_val) *im_out=0;
  2105. }
  2106. }
  2108. inline void set_zero_to_undefined(uchar* im, uchar* im_out, uchar undefined_val, int dataSize)
  2109. {
  2110. for (int i=0; i<dataSize; ++i,++im,++im_out)
  2111. {
  2112. if (*im == 0) *im_out=undefined_val;
  2113. }
  2114. }
  2118. inline void copyMMX(void* imDest, const void* imSrc, int dataSize)
  2119. {
  2120. __asm {
  2122. mov ecx, dataSize
  2123. shr ecx, 3
  2125. mov eax, imSrc
  2126. mov ebx, imDest
  2127. sub ebx, 8
  2129. pixel_loop:
  2130. movq mm1, [eax]
  2131. add ebx, 8
  2133. movq [ebx], mm1
  2134. add eax, 8
  2136. dec ecx
  2137. jnz pixel_loop
  2139. jmp done
  2141. done:
  2142. emms
  2143. }
  2144. }
  2146. inline void copySSE(void* imDest, const void* imSrc, int dataSize)
  2147. {
  2148. __asm {
  2150. mov ecx, dataSize
  2151. shr ecx, 4
  2153. mov eax, imSrc
  2154. mov ebx, imDest
  2155. sub ebx, 16
  2157. pixel_loop:
  2158. movdqa xmm1, [eax]
  2159. add ebx, 16
  2161. movdqa [ebx], xmm1
  2162. add eax, 16
  2164. dec ecx
  2165. jnz pixel_loop
  2167. jmp done
  2169. done:
  2170. emms
  2171. }
  2172. }
  2174. inline void setMMX(float* imDest, const float value, int dataSize)
  2175. {
  2176. __asm {
  2177. // make 4 copies of the constant 'value' in xmm0
  2178. movss xmm0, value
  2179. movss xmm1, xmm0
  2180. unpcklps xmm0, xmm1        
  2181. movlhps xmm0, xmm0
  2183. mov ecx, dataSize
  2184. shr ecx, 2
  2186. mov ebx, imDest
  2188. pixel_loop:
  2189. movaps [ebx], xmm0
  2190. add ebx, 16
  2192. dec ecx
  2193. jnz pixel_loop
  2195. jmp done
  2197. done:
  2198. emms
  2199. }
  2200. }
  2202. inline void setMMX(char* imDest, const char value, int dataSize)
  2203. {
  2204. __asm {
  2205. // setup mm0 with 8 copies of 'value'
  2206. mov al,  value
  2207. mov ah,  al
  2208. mov bx,  ax
  2209. shl eax,  16
  2210. mov ax,  bx
  2211. movd mm0,  eax
  2212. movd mm1,  eax
  2213. punpckldq mm0,  mm1
  2216. mov ecx, dataSize
  2217. shr ecx, 3
  2219. mov ebx, imDest
  2221. pixel_loop:
  2222. movq [ebx], mm0
  2223. add ebx, 8
  2225. dec ecx
  2226. jnz pixel_loop
  2228. jmp done
  2230. done:
  2231. emms
  2232. }
  2233. }
  2235. /*
  2236. void copyRGBAtoRGB(const uchar* imSrc, uchar* imred,uchar* imgreen,uchar* imblue, int dataSize)
  2237. {
  2238. __asm {
  2240. mov esi, dataSize
  2241. shr esi, 3
  2243. mov eax, imSrc
  2244. mov ebx, imred
  2245. mov ecx, imred
  2246. mov edx, imred
  2248. pixel_loop:
  2249. movq mm1, [eax]
  2252. movq [ebx], mm1
  2254. add eax, 8
  2255. add ebx, 8
  2256. add ecx, 8
  2257. add edx, 8
  2259. dec esi
  2260. jnz pixel_loop
  2262. jmp done
  2264. done:
  2265. emms
  2266. }
  2267. }*/
  2269. inline void multiply(uchar* im, float fact, int imageSize)
  2270. {
  2271. __asm {
  2273. mov ecx, imageSize
  2274. shr ecx, 3
  2276. // make 4 copies of the constant 'fact' in xmm0
  2277. movss xmm0, fact
  2278. movss xmm1, xmm0
  2279. unpcklps xmm0, xmm1        
  2280. movlhps xmm0, xmm0
  2283. mov eax, im
  2284. pxor mm7,mm7 // mm7 = 0 
  2286. pixel_loop:
  2287. movq mm1, [eax]
  2288. movq mm2, mm1
  2290. punpcklbw mm2, mm0
  2291. punpckhbw mm1, mm0
  2293. movq mm3,mm2
  2294. punpckhwd mm3, mm0
  2295. punpcklwd mm2, mm0
  2297. movq mm4,mm1
  2298. punpcklwd mm4, mm0
  2299. punpckhwd mm1, mm0
  2301. // here, the first 8 bytes are in d-words [mm1 mm4 mm3 mm2]
  2302. // --------
  2303. cvtpi2ps xmm3, mm3 // put mm3 in low part of xmm3
  2304. cvtpi2ps xmm2, mm2 // put mm2 in low part of xmm2
  2306. movlhps xmm2, xmm3 // xmm2 = [xmm3(low part) xmm2(low part)]
  2308. mulps xmm2, xmm0
  2310. cvtps2pi mm2, xmm2 // convert low 2 floats from xmm2 to mm2
  2311. movhlps xmm3,xmm2 // mov high 2 floats from xmm2 to low 2 floats in xmm3
  2312. cvtps2pi mm3, xmm3 // convert low 2 floats from xmm3 to mm3
  2314. packssdw mm2, mm3 // mm2 = (word)[mm2 mm3]
  2316. // --------
  2317. cvtpi2ps xmm4, mm4
  2318. cvtpi2ps xmm1, mm1
  2320. movlhps xmm4, xmm1
  2322. mulps xmm4, xmm0
  2324. cvtps2pi mm4, xmm4 // convert low 2 floats from xmm4 to mm4
  2325. movhlps xmm1,xmm4 // mov high 2 floats from xmm4 to low 2 floats in xmm1
  2326. cvtps2pi mm1, xmm1 // convert low 2 floats from xmm1 to mm1
  2328. packssdw mm4, mm1 // mm4 = (word)[mm1 mm4]
  2331. // ------
  2332. packuswb mm2, mm4 // mm2 = [[mm4] [mm2]] = [mm1 mm4 mm2 mm3]
  2333. movq [eax], mm2
  2335. add eax, 8
  2336. dec ecx
  2337. jnz pixel_loop
  2339. jmp done
  2341. done:
  2342. emms
  2344. }
  2345. }
  2347. inline void multiply(const uchar* imSrc, uchar* imDest, float fact, int imageSize)
  2348. {
  2349. __asm {
  2351. mov ecx, imageSize
  2352. shr ecx, 3
  2354. // make 4 copies of the constant 'fact' in xmm0
  2355. movss xmm0, fact
  2356. movss xmm1, xmm0
  2357. unpcklps xmm0, xmm1        
  2358. movlhps xmm0, xmm0
  2361. mov eax, imSrc
  2362. mov ebx, imDest
  2363. pxor mm7,mm7 // mm7 = 0 
  2365. pixel_loop:
  2366. movq mm1, [eax]
  2367. movq mm2, mm1
  2369. punpcklbw mm2, mm0
  2370. punpckhbw mm1, mm0
  2372. movq mm3,mm2
  2373. punpckhwd mm3, mm0
  2374. punpcklwd mm2, mm0
  2376. movq mm4,mm1
  2377. punpcklwd mm4, mm0
  2378. punpckhwd mm1, mm0
  2380. // here, the first 8 bytes are in d-words [mm1 mm4 mm3 mm2]
  2381. // --------
  2382. cvtpi2ps xmm3, mm3 // put mm3 in low part of xmm3
  2383. cvtpi2ps xmm2, mm2 // put mm2 in low part of xmm2
  2385. movlhps xmm2, xmm3 // xmm2 = [xmm3(low part) xmm2(low part)]
  2387. mulps xmm2, xmm0
  2389. cvtps2pi mm2, xmm2 // convert low 2 floats from xmm2 to mm2
  2390. movhlps xmm3,xmm2 // mov high 2 floats from xmm2 to low 2 floats in xmm3
  2391. cvtps2pi mm3, xmm3 // convert low 2 floats from xmm3 to mm3
  2393. packssdw mm2, mm3 // mm2 = (word)[mm2 mm3]
  2395. // --------
  2396. cvtpi2ps xmm4, mm4
  2397. cvtpi2ps xmm1, mm1
  2399. movlhps xmm4, xmm1
  2401. mulps xmm4, xmm0
  2403. cvtps2pi mm4, xmm4 // convert low 2 floats from xmm4 to mm4
  2404. movhlps xmm1,xmm4 // mov high 2 floats from xmm4 to low 2 floats in xmm1
  2405. cvtps2pi mm1, xmm1 // convert low 2 floats from xmm1 to mm1
  2407. packssdw mm4, mm1 // mm4 = (word)[mm1 mm4]
  2410. // ------
  2411. packuswb mm2, mm4 // mm2 = [[mm4] [mm2]] = [mm1 mm4 mm2 mm3]
  2412. movq [ebx], mm2
  2414. add eax, 8
  2415. add ebx, 8
  2416. dec ecx
  2417. jnz pixel_loop
  2419. jmp done
  2421. done:
  2422. emms
  2424. }
  2425. }