3D图形编程

开发平台：
C/C++

subPixel.cpp：源码内容
							/*************************************************************************** 
*
* Copyright 2000 by David Demirdjian.   All rights reserved. 
*  
* Developed  by David Demirdjian
*  
* Permission to use, copy, or modify this software and  its documentation 
* for  educational  and  research purposes only and without fee  is hereby 
* granted, provided  that this copyright notice and the original authors's 
* names appear  on all copies and supporting documentation.  If individual 
* files are  separated from  this  distribution directory  structure, this 
* copyright notice must be included.  For any other uses of this software, 
* in original or  modified form, including but not limited to distribution 
* in whole or in  part, specific  prior permission  must be  obtained from 
* MIT.  These programs shall not  be  used, rewritten, or  adapted as  the 
* basis  of  a  commercial  software  or  hardware product  without  first 
* obtaining appropriate  licenses from David Demirdjian.  The author makes 
* no representations about the suitability of this software for any purpose.  
* It is provided "as is" without express or implied warranty. 
*  
**************************************************************************/
#include "stdafx.h"
#include "stereoMatching.h"
#include "processingMMX.h"
#include <stdio.h>
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
int subpixelDisparity(float *scorePrev, float *scoreInit, float* scoreNext,
					  float *Delta,  int l)
{
	for (int i=0; i<l; ++i,++scorePrev,++scoreInit,++scoreNext,++Delta) {
		if (*Delta>0) {
			float A2 = *scoreNext-*scoreInit;
			float A0 = *scoreInit-*scorePrev;
			float dd = 0.5f*(A0+A2)/(A0-A2);
			/*if (fabs(dd)>1)  {
				float err0 = *scorePrev;
				float err1 = *scoreInit;
				float err2 = *scoreNext;
				float err4 = 1.0f;
			}*/
			*Delta += dd;
		} else *Delta=0;
	}
	return 1;
}
int subpixelDisparity_mmx(float *scorePrev, float *scoreInit, float* scoreNext,
						  float *Delta,  int l)
{
	const float two_float = -2.0f;
	if (l < 8) return 0;              // image size must be at least 8 bytes 
  __asm 
  {		
	  		// make 4 copies of the constant 'radius' in xmm0
		movss	xmm0, two_float
		movss	xmm1, xmm0
		unpcklps	xmm0, xmm1        
		movlhps		xmm0, xmm0
        mov eax, scorePrev     
        mov ebx, scoreInit
		mov ecx, scoreNext
        mov edi, Delta    
        mov	edx, l   
        shr	edx, 2	
	
align 16
inner_loop:
		movaps	xmm1,[eax]	// xmm1=scorePrev
		movaps	xmm3,[ecx]	// xmm3=scoreNext
		movaps  xmm4, xmm1  // xmm4 = scorePrev
		movaps	xmm2,[ebx]	// xmm2=scoreInit
		subps   xmm4, xmm3	// xmm4 = scorePrev - scoreNext
							// xmm4 = delta = scorePrev - scoreNext
		// scoreInit = -2*(scorePrev + scoreNext -2*scoreInit)
		movaps  xmm5, [edi]
		mulps   xmm2, xmm0		// nspsbMpy1(-2.0, scoreInit, nbPoints);
		addps   xmm2, xmm1		// nspsbAdd2(scorePrev, scoreInit, nbPoints);
		addps   xmm2, xmm3		// nspsbAdd2(scoreNext, scoreInit, nbPoints);
		mulps   xmm2, xmm0		// warning : nspsbMpy1(-2.0, scoreInit, nbPoints);
				
		// delta = delta/scoreInit
		divps  xmm4, xmm2
		// delta += dispFloat
		subps xmm5, xmm4
		movaps  [edi], xmm5
        add eax,16         
        add ebx,16    
		add ecx, 16
        add edi,16		
        dec edx      
        jnz inner_loop    
        emms   		
  }
	
  return 1;
}
void StereoMatching::getList_subPixel(StereoImage* sdata, int sc, const StereoBuffer* sb)
{
	if (sb==NULL) sb = sbuffer+sc;
	// create idx list of valid  pixels
	sdata->createValidPixelsList(maskSizeY/2+1);
	// estimate subpixel disparities for each valid pixel
	getList_subPixel(sdata->x, sdata->y, 
					 sdata->depth_float_list, sdata->m_nGood3DPoints,
					 sb, sdata->imDepth8u, sc);
	// build imDepth32f from x, y, depth_float_list and m_nGood3DPoints
	sdata->generateDepth32f();
	sdata->generateDepth8uFromDepth32f();
}
// does not check whether disp is in the good range
// estimate 'delta', sub-pixels disparities of pixel lists (x,y) of image depth
void StereoMatching::getList_subPixel(const short* x, const short* y, 
									  float* depth_list, int nbPoints,
									  const StereoBuffer* sb, uchar* imDepth_used,
									  int sc)
{
	if (nbPoints>0) {
		int i, idx;
		int maxDisparity = (int)(nbDepth/pow(2, sc));
		float* ptPrev=scorePrev, *ptInit=scoreInit, *ptNext=scoreNext;
		uchar disp;
		float* ptDelta = depth_list;
		
		// -----------------------------------------
		// prepare lists of disparity and SAD scores
		for (i=0; i<nbPoints; ++i,++x,++y,++ptPrev,++ptInit,++ptNext,++ptDelta) {
			// set index
			idx = *x + sb->width*(*y);
			// set current disparity
			*ptDelta = disp = __max(0, __min( maxDisparity-1, *(imDepth_used + idx)));
			// set SAD scores for (disp-1, disp and disp+1)
			*ptInit = *(sb->buff[disp] + idx);
			*ptPrev = *(sb->buff[__max(disp-1,0)] + idx);
			*ptNext = *(sb->buff[__min(disp+1,maxDisparity-1)] + idx);
			/*if ((*ptInit)*2 >= (*ptPrev + *ptNext)) {
				*ptPrev = *ptNext = *ptInit+20;
			}*/
		}
		// -----------------------------------------
		// compute sub-pixel disparities
		subpixelDisparity(scorePrev, scoreInit, scoreNext, depth_list, nbPoints);
	}
}

						